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ABSTRACT 


The  major  problem  addressed  by  this  research  is  how  to  plan  a  safe  motion  for 
autonomous  vehicles  in  a  two  dimensional,  rectilinear  world.  With  given  start  and  goal 
configurations,  the  planner  performs  motion  planning  which  will  lead  a  vehicle  to  achieve 
its  task  safely.  During  the  planning,  in  addition  to  the  safety  consideration,  motion’s 
smoothness  is  also  taken  into  account. 

The  approach  taken  was  to  divide  whole  motion  planning  task  into  two  layers.  The 
top  layer  finds  a  global  path  by  decomposing  the  free  space  into  convex  regions,  then 
searching  for  an  optimal  global  path  class.  The  bottom  layer  performs  local  motion 
planning  which  further  subdivides  the  planning  problem  into  mid-portion  and  end-portion 
motion  planning.  The  local  motion  planning  is  carried  out  region  by  region  along  the  global 
path  class.  As  results,  simple  motion  instructions  are  generated  for  each  region. 

For  execution  of  planned  motion,  a  software  system.  Model-based  Mobile  robot 
Language  (MML-11),  was  developed.  This  easy-to-use  robot  language  provides  users  a 
convenient  tool  to  program  their  applications  through  its  function  library. 

The  results  of  the  research  were  implemented  in  MML-11  and  tested  on  an 
experimental  robot  Yamabico-11  successfully. 
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I.  INTRODUCTION 


Imagine  an  autonomous  robot  vehicle  moving  indoors.  The  mission  is  to  provide 
for  its  navigation.  What  are  the  problems  that  needs  to  be  solved  to  accomplish  its  mission 
and  how  can  those  problems  be  solved?  These  have  been  major  sources  of  research  topics 
in  robotics.  We  refer  to  these  problems  as  motion  planning  problems. 

The  basic  motion  planning  problem  was  simplified  and  defined  by  Jean  C.  Latombe 
as  follows: 

Let  Abe  a  single  rigid  object  --  the  robot  —  moving  in  a  Euclidean  space  W, 
called  workspace,  represented  as  91",  where  n  =  2  or  3. 

Let  (Bj . 'Bq  be  fixed  rigid  objects  distributed  in  W.  The  l!B,’s  are  called 

obstacles. 

Assume  that  both  the  geometric  descriptions  of  A  ^i>  •••,  and  the  locations 
of  the  !Bj’s  in  Tl?are  accurately  known.  Assume  further  that  no  kinematic  constrains 
limit  the  motion  of  A 

The  problem:  Given  an  initial  position  and  orientation  and  a  goal  position  and 
orientation  of  in  *11^  generate  a  path,  specifying  a  continuous  sequence  of  positions 
and  orientations  of  A,  avoiding  contact  with  the  lB,’s,  starting  at  the  initial  position  and 
orientation,  and  terminating  at  the  goal  position  and  orientation.  Report  failure  if  no 
such  path  exists.  [1] 

This  definition  was,  of  course,  oversimplified.  However,  it  stated  the  essential  idea 
of  motion  planning.  A  formal  statement  of  the  problem,  known  as  the  configuration  space 
formulation,  then  was  presented  in  the  book  [1  p.7-  1 1]  where  the  terms,  configuration, 
configuration  space  (Cspace),  obstacle,  configuration  obstacle  (Cobstacle),  free  space, 
path,  and  free  path,  are  defined  more  precisely. 
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A.  REVffiW  OF  OTHER  MOTION  PLANNING  APPROACHES 


Many  different  methods  have  been  developed  for  motion  planning  over  the  years. 
Some  of  them  are  applicable  to  a  wide  variety  of  motion  planning  problem,  whereas  others 
have  limited  applicability.  Latombe  summarized  the  general  approaches  to  the  motion 
planning  problem  into  three  categories:  roadmap,  cell  decomposition,  and  potential  field 
[1].  We  will  briefly  introduce  these  approaches  and  summarize  them  below. 

I.  Roadmap 

Roadmap  approaches  attempt  to  capture  the  connectivity  of  the  set  of  free 
configurations  of  a  robot  in  the  form  of  a  network  of  one-dimensional  curves  called 
roadmap  [2].  Once  a  roadmap  is  constructed,  it  is  used  as  a  set  of  standardized  paths. 
Motion  planning  is  thus  reduced  to  connecting  the  initial  and  goal  configurations  to  the 
roadmap  and  searching  the  network  for  a  path  between  these  two  configurations. 

Various  methods  based  on  this  general  idea  have  been  proposed.  The  best-known 
types  of  roadmaps  are  visibility  graph,  Voronoi  diagram,  freeway  net  and  silhouette.  The 
visibility  graph  is  one  of  the  earliest  path  planning  methods.  In  two-dimensional  Cspace, 
this  approach  consists  of  considering  all  the  vertices  of  the  Cobstacles  and  the  start  and  goal 
configurations  as  the  roadmap  nodes,  while  the  links  are  all  the  line  segments  connecting 
two  nodes  that  do  not  intersect  any  of  the  Cobstacles.  The  visibility  graph  can  be  searched 
for  the  shortest  path  between  start  and  goal  configurations. 

The  Voronoi  diagram  is  defined  as  the  set  of  points  that  are  equidistant  from  two  or 
more  objects  [3].  The  start  and  goal  configurations  are  retracted  in  the  diagram.  A  path  is 
searched  in  the  diagram  between  start  and  goal  configurations.  A  comprehensive  survey  of 
the  Voronoi  diagram  is  presented  in  Aurenhammer  [4].  The  advantage  of  this  diagram  is 
that  it  yields  free  paths  which  tend  to  maximize  the  clearance  between  the  robot  and  the 
obstacles. 

The  freeway  net  method  generates  paths  along  generalized  cones  between  the 
obstacles  [5].  The  silhouette  method  uses  techniques  from  differential  topology.  It  projects 
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an  object  in  a  higher-dimensional  space  to  a  lower-dimensional  space  and  then  traces  out 
the  boundary  curves  of  the  projection,  which  is  called  silhouette.  The  silhouette  ciuves  are 
recursively  projected  to  lower-dimensional  space,  until  they  become  one-dimensional 
lines.  Then  the  curves  are  connected  at  places  where  new  silhouette  curves  appear  or 
disappear  using  linking  curves.  This  method  is  developed  to  find  a  path  from  a  graph  of 
one-dimensional  curves.  It  is  mostly  used  in  theoretical  algorithms  analyzing  complexity, 
rather  than  in  developing  practical  algorithms  [6]. 

2.  Cell  Decomposition 

The  cell  decomposition  approaches  have  been  very  widely  used  and  are  based  on 
decomposing  (either  exactly  or  approximately)  the  set  of  free  configurations  into  simple 
non-overlapping  regions  called  cells.Vat  adjacencies  of  these  cells  are  then  represented  in 
a  connectivity  graph.  A  collision-free  path  between  the  start  and  the  goal  configuration  of 
the  robot  is  found  by  first  identifying  the  two  cells  containing  the  start  and  goal 
configurations  and  then  connecting  them  with  a  sequence  of  connected  cells. 

Cells  can  be  object-dependent  or  object-independent.  In  object-dependent 
decomposition,  boundaries  of  obstacles  are  used  to  generate  the  cell  boundaries,  and  the 
union  of  firee  cells  exactly  defines  the  free  space.  The  number  of  cells  is  small,  but  the 
complexity  of  decomposition  is  high  [3]  [7].  In  an  object-independent  decomposition,  the 
Cspace  is  prepartitioned  into  cells  of  a  simple  shape,  and  each  cell  is  tested  to  determine 
whether  it  is  inside  or  outside  of  Cobstacles  [8]  [9]. 

3.  Potential  Field 

The  potential  field  methods  generally  employ  repulsive  potential  fields  around 
obstacles  and  an  attractive  field  around  the  goal.  The  gradient  of  the  combined  potential 
guides  the  path  away  from  the  obstacles  and  toward  the  goal.  A  historical  review  of  the 
potential  field  approach  can  be  found  in  [10].  The  major  drawback  with  these  methods  is 
that  the  potential  function  will  often  lead  the  path  to  some  local  minimum  from  which  it 
cannot  escape  and  therefore  causes  the  planner  to  fail  [11]. 
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4.  Layered  Approach 

Mobile  robots  are  likely  based  upon  carts  with  wheels  for  steering  and  locomotion 
[12].  It  is  not  possible  to  follow  an  arbitrary  path.  Many  constraints  need  to  be  taken  into 
account  in  planning  paths  for  mobile  robots.  Based  on  the  motion  planning  categories 
stated  above,  numerous  algorithms  have  been  developed  for  autonomous  robots  over  a 
period  of  years.  Laumond  extended  the  basic  motion  planning  problem  to  the  case  of  a 
point  robot  with  kinematic  constraints  [13].  He  developed  a  method  to  break  down  the 
planning  problem  into  two  phases.  In  the  first  phase,  the  problem  is  solved  by  finding  a 
collision-free  path  while  ignoring  the  orientations  of  robot’s  start  and  goal  configurations. 
Then  in  second  phase  the  path  is  transformed  into  a  topologically  equivalent  collision-free 
path  using  arcs  and  tangent  line  segments.  The  number  of  reversals  in  the  path  is  not  limited 
and  the  path  involving  reversals  is  not  smooth. 

Another  approach  to  this  problem  breaks  up  the  free  space  into  convex  cells,  and 
computes  intermediate  configurations  at  the  border  of  every  adjacent  pair  of  cells  [14]. 
Because  of  the  characteristics  of  convex  cells,  a  simple  procedure  is  devised  to  connect  the 
intermediate  configurations  with  arcs  and  tangent  lines.  Reverse  motions  are  allowed 
throughout  the  path  in  this  research.  Simulation  results  showed  that  the  solution  path 
involves  many  unnecessary  reverse  motions  even  in  a  spacious  environment.  Moreover,  the 
motions  for  the  initial  and  final  stages  were  not  described  clearly. 

One  of  the  closely  related  research  is  to  develop  algorithms  for  motion  planning 
with  different  layers  [5].  This  method  proposed  using  Spine  net  to  construct  a  global  path. 
Then  straight  line  segments  and  circular  arcs  were  used  to  plan  the  detailed  motion.  The 
Cubic  spiral  was  adopted  to  smooth  the  local  motion.  This  method  looks  complete  but  is 
hard  to  implement.  The  other  global  motion  planning  and  local  motion  planning  ideas  can 
be  found  in  other  research  reports.  Some  of  these  focus  on  motion  planning  for 
manipulators  [1 1].  Some  of  them  provide  general  ideas  [3]  [15]  [9]. 

The  most  recent  research  in  layered  motion  planning  is  presented  by  Kovalchik,  J. 
[7]  in  otu:  research  group.  The  method  used  in  his  research  is  to  first  decompose  the  fi’ee 
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space  into  K-regions.  After  this  a  connectivity  graph  is  built  and  searched  to  find  a  global 
path  firom  the  region  that  contains  the  start  configuration  to  the  region  containing  the  goal 
configuration.  A  bidirectional  motion  planning  method  is  also  introduced  in  this  research. 
In  short,  this  research  concentrates  on  the  global  path  planning  which  will  be  adopted  in 
this  dissertation  for  the  top  layer  of  the  autonomous  vehicle  motion  planning. 

B.  SCOPE  OF  DISSERTATION 

This  dissertation  solves  the  motion  planning  problem  under  the  assumption  to  be 
described  in  Section  C  of  this  chapter.  Our  focus  is  on  planning  the  motion  as  a  physical 
approach.  The  algorithms  for  motion  planning  to  be  taken  by  real  robot  are  deliberatively 
developed.  Simulations  are  done  to  verify  the  algorithms  either  individually  or  globally. 

After  this  a  high-level  motion  specification  language.  Model-based  Mobile-robot 
Language  (MML-11),  which  is  suitable  to  describe  the  solutions  to  the  motion  planning 
problem,  is  developed  as  the  implementation  and  verification  of  the  algorithms.  The 
dissertation  also  includes  the  description  of  MML-11. 

C.  MOTION  PLANNING  PROBLEM 

1.  Problem  Statement 

The  world  space  Wfot  the  motion  planning  problem  in  this  dissertation  is  a  two- 

dimensional  plane  91^  on  which  a  global  Cartesian  coordinate  system  is  defined.  The 
obstacles  are  closed  subsets  of  W.  In  this  dissertation  obstacles  are  assumed  to  be  simple, 
rectilinear,  and  polygonal.  The  free  space,  Free( is  the  complement  of  the  union  of  all 
obstacles  in  the  world  space  W. 

The  vehicle  in  the  dissertation  refers  to  a  rigid-body  robot  which  has  fixed  size. 
(During  the  research  we  will  use  Yamabico-11  for  experiments  although  the  algorithms 
will  be  suitable  for  robots  with  any  size). 

A  configuration  q  in  this  dissertation  is  defined  as  a  triple  q  =  (p,  6,  k),  where  p 
indicates  the  position  (x,  y)  in  the  global  Cartesian  coordinate  system,  0  is  the  orientation 
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related  to  the  x-axis  of  the  global  coordinate  system  [16],  and  k  is  the  specified  curvature. 
The  configuration  defined  in  this  dissertation  is  normally  used  to  describe  the  robot’s 
instantaneous  status,  either  it  is  stationary  or  moving.  This  configuration  is  especially 
useful  for  specifying  a  path.  For  instance,  if  we  use  ^  =  ((x,  y),  6,  k)  to  specify  a  line,  this 
line  passes  through  the  point  at  (x,  y)  and  with  orientation  0.  When  the  curvature  element 
is  ^  =  0,  it  is  specifying  a  straight  line,  otherwise  it  is  a  curve. 

The  motion  of  a  vehicle  is  subject  to  nonholonomic  and  kinematic  constraints.  That 
is,  the  vehicle  is  able  to  perform  both  forward  and  backward  motion  but  not  sideways 
motion.  The  robot’s  orientation  and  curvature  are  continuous.  There  is  an  upper  limit  in  its 
curvature  when  a  turn  is  taken. 

The  problem  to  be  solved  in  this  dissertation  is:  In  a  given  world  W,  with  free  space 
Free(W),  let  and  Qg  be  the  start  and  goal  configurations,  respectively,  lying  completely 

inside  free  space.  Let  .J?be  the  wheeled  mobile  robot  vehicle.  Given  a  mission  to  move  from 
Qs  to  Qg,  this  research  is  to  plan  a  safe  motion  symmetrically  for  the  vehicle  under  the  given 
constraints,  to  achieve  its  mission. 

2.  Layered  Motion  Planning 

As  stated  in  Section  A,  numerous  methods  have  been  developed  for  motion 
planning.  These  methods  are  variations  of  a  few  general  approaches:  road  map,  cell 
decomposition,  potential  field,  and  mathematical  programming  [3]  [1].  Some  of  them  are 
applicable  to  a  wide  variety  of  motion  planning  problems,  whereas  others  have  a  limited 
applicability.  Unfortunately,  none  of  them  are  complete  in  the  sense  of  practical  use  for 
solving  the  motion  planning  problem  defined  in  this  dissertation.  For  example, 
nonholonomic  constraints  and  kinematic  constraints  were  not  taken  into  consideration  in 
many  approaches.  In  particular,  the  robot’s  motions  in  the  area  of  the  start  or  goal 
configurations  are  more  restricted,  and  so  require  more  deliberative  planning.  Not  all 
robotics  systems  proposed  for  motion  planning  are  developed  to  address  this  consideration. 
Furthermore,  most  of  research  in  motion  planning  for  mobile  robot  is  theoretically  valuable 
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but  not  practically  useful.  For  these  reasons,  we  propose  a  new  approach  which  divides  the 
motion  planning  into  two  layers:  the  global  path  planning  and  the  local  motion  planning, 
as  shown  in  Figure  1.1. 


^  Top  Layer: 


Path  Class  Representation 


Global  Path  Planning 


Motion 

Planning 


< 


Path  Class  Determination 


Bottom  Layer: 


Local  Motion  Planning 


Mid-Portion  Motion 
Planning 

End-Portion  Motion 
Planning 


Figure  1.1:  Layered  Structure  of  Motion  Planning 


The  top  layer,  global  path  planning,  starts  with  decomposition  of  the  free  space  of 
the  given  world  into  K-regions.  Then  a  connectivity  graph  is  built  and  searched  to 
determine  path  classes  which  classify  equivalent  paths.  At  completion  of  motion  planning 
in  this  layer,  one  path  class  among  all  distinct  classes  is  determined.  This  path  class, 
represented  by  a  sequence  of  alternative  region-borders  called  a  crossing  sequence  [7], 
specifies  the  direction  of  a  possible  path  for  a  robot,  which  will  continue  to  plan  its  detailed 
motion  in  next  layer.  The  details  of  top  layer  motion  planning  wiU  be  reviewed  in  Chapter 

n. 

With  the  global  path  determined,  the  motion  planner  at  the  bottom  layer  then  takes 
the  general  direction  guided  by  the  global  path  to  direct  its  local  motion  planning.  The  task 
of  this  layer  is  to  produce  a  smooth  collision-free  motion  for  the  robot.  We  further  divide 
this  layer  into  two  types  of  motion  planning:  the  mid-portion  motion  planning  and  the 
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end-portion  motion  planning.  In  mid-portion  motion  planning,  the  robot’s  motion  will  be 
planned  region-by-region  along  the  global  path,  excluding  the  regions  in  or  near  the  first 
and  last  regions.  The  end-portion  motion  planning  determines  the  motion  of  starting  or 
ending  from  start  or  final  configurations  which  are  called  initial  motion  planning  and  final 
motion  planning  respectively.  Both  types  of  local  motion  planning  are  done  under 
nonholonomic  and  kinematic  constraints.  The  steering  function  and  forerunner  simulation 
are  powerful  methods  to  find  solutions  in  this  layer.  The  bottom  layer  motion  planning  will 
be  discussed  in  detail  in  Chapters  ID,  IV  and  V.  The  structure  of  motion  planning  and 
motion  execution  is  shown  in  Figure  1.2. 


W 

qs 

qg 


Figure  1.2:  Structure  of  Motion  Planning  and  Execution. 

3.  Assumptions 

This  research  is  based  on  the  following  assumptions: 

•  Two  dimensional  world:  The  world  space  is  a  flat  planar  world  with  obstacles. 
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The  floor  is  on  the  x,  y  plane. 

•  Rectilinear  obstacles:  All  obstacles  in  the  dissertation  refer  to  rectilinear, 
polygonal  obstacles  which  have  their  edges  perpendicular  to  the  x,  y  axis  of  the 
coordinate  system. 

•  Stationary  environment:  The  environment  in  this  dissertation  refers  to  a 
stationary  world  space. 

•  Gross  motion  planning:  We  assume  that  free  space  is  much  wider  than  the 
objects’  size  with  allowance  for  the  positional  error  of  the  robot.  This  ensures 
that  position  error  will  not  cause  unexpected  collisions  while  executing  the 
collision-free  paths  generated  by  the  motion  planner.  The  motion  planning 
under  this  assumption  is  called  gross  motion  planning  [3]. 

•  No  localization  error:  In  this  dissertation,  we  assume  that  the  localization  error 
has  been  corrected.  Thus  the  localization  error  does  not  exist  during  the 
execution  of  the  planned  motion. 

•  The  motion  is  subject  to  constraints  of  robot’s  kinematic  and  nonholonomic 
characteristics. 

•  The  robot  is  a  rigid-body  autonomous  vehicle. 

D.  ORGANIZATION  OF  DISSERTATION 

This  dissertation  provides  a  solution  to  the  motion  planning  problem  for 
autonomous  vehicles.  It  includes  descriptions  of  existing  planning  approaches,  design  of  a 
robotic  language,  and  implementations.  In  addition,  an  experimental  robot,  Yamabico-11, 
which  plays  a  significant  role  during  the  entire  project,  will  be  introduced.  The  dissertation 
is  organized  as  follows: 

Chapter  I  gives  an  overview  of  the  dissertation.  It  reviews  the  general  motion 
planning  approaches,  defines  the  scope  of  the  dissertation,  and  states  the  problem  to  be 
solved. 
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Chapter  II  describes  the  global  path  planning.  We  review  the  theory  of  homotopy 
path  classes  and  the  theory  of  K-decomposition  that  will  be  adopted  in  the  top-layer 
planning.  Then  a  path-finding  method  for  building  the  global  path  represented  by  K-regions 
is  discussed. 

In  Chapter  HI,  we  break  down  the  local  motion  planning  into  two  types  of  planning: 
end-portion  motion  planning  and  mid-portion  motion  planning.  We  present  the  analysis  of 
local  motion  planning  tools  to  be  used  in  this  dissertation.  The  characteristics  of  steering 
function  are  studied  in  detail.  The  concepts  of  Forerunner  simulation  and  reverse  path  are 
described.  After  then,  we  discuss  the  steps  to  be  taken  in  local  motion  planning. 

Chapter  IV  discusses  mid-portion  motion  planning  in  detailed.  The  planning 
methods  for  various  t5^es  of  K-region  on  the  path  are  identified  and  analyzed.  The 
algorithms  for  planning  the  robot’s  motion  in  regions  other  than  the  initial  and  final  ones 
are  developed. 

Chapter  V  gives  a  detailed  description  of  end-portion  motion  planning  including  the 
initial  motion  planning  and  final  motion  planning.  Planning  tools  are  described  prior  to  the 
discussion  of  planing  details. 

Chapter  VI  introduces  the  hardware  of  the  experimental  robot  Yamabico-1 1. 

Chapter  Vn,  Vin,  and  IX  describes  the  design  of  a  robotic  software  system  -- 
Model-based  Mobile  robot  Language,  MML-11.  Chapter  VIII  states  the  design  of  robot 
real  time  operating  system.  In  Chapter  IX,  the  language  specifications  and  software 
architecture  are  described. 

Chapter  X  presents  a  sensor-based  approach  to  motion  control 

Chapter  XI  and  Xn  present  results  and  conclusions. 
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11.  GLOBAL  PATH  PLANNING 


A.  HOMOTOPY 

We  consider  a  two  dimensional  world,  W,  with  holes.  A  hole  is  an  obstacle  for  a 
robot.  A  free  space  Free(W)  is  the  complement  of  the  union  of  all  holes.  There  might  be  a 
hole  among  them  which  completely  surrounds  the  free  space.  A  hole  with  this  property  is 
said  to  be  inverted.  Every  free  space  is  a  connected  subset  of  W  (Figure  2.1). 


(a)  Without  Inverted  Hole  (b)  With  Inverted  Hole 

Figure  2.1:  Two-Dimensional  World  Space  with  Holes 

A  directed  curve  or  directed  path  H  is  represented  by  a  continuous  function/;  [0, 
1]  -»  Free(W).  The  two  points  f(0)  and  f(l)  are  called  endpoints  of  H.  The  path  is  said  to 
join  the  endpoints.  Obviously,  there  are  infinitely  many  paths  joining  given  two  points  S 
and  G  in  Free(W)  (Figure  2.2).  Li  this  figure,  paths  Hi  and  02  are  somewhat  similar  and  so 
are  paths  113  and  114.  However,  Hj  and  03  are  not.  This  concept  was  formally  defined  in 
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the  field  of  algebraic  topology  [17]  and  will  be  followed.  Two  paths  11  and  11’  (defined  by 
/  and  f  respectively)  are  said  to  be  homotopic  if  and  only  if  there  exists  a  continuous 
function  (j):  [0,1]  x  [0,1]  —>  Free(W)  such  that 

1. (|)(0,0=/(0)forallte  [0, 1], 

2.  (t)(l,0=/(l)foraUt€  [0, 1], 

3.  (l)(s,  0)  =f(s)  for  all  ^  €  [0, 1],  and 

4.  1)  =f(s)  for  all  ^  e  [0, 1]. 


Figure  2.2:  Homotopic  Paths 


Informally  speaking,  ITi  can  be  continuously  transformed  into  112,  without  running 
over  any  holes,  with  both  endpoints  fixed.  So  are  paths  113  and  114.  If  two  paths  11  and  11’ 
are  equivalent,  we  write 

n  =  n'  (Eq2.1) 

Therefore  in  Figure  2.2,  rij  =  112  ^3  -  ^4,  There  are  a  countable  number  of 

equivalence  classes  of  paths,  even  in  a  world  with  a  finite  number  of  holes. 
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B.  OVERVIEW  OF  CONVEX  POLYGONAL  K-REGIONS  THEORY 


The  ability  to  decompose  a  world  into  regions  which  capture  the  topology  of  the 
space  in  terms  of  distinct  homotopy  classes  and  which  provide  useful  data  for  the  local 
motion  planner  is  important  in  global  path  planning.  Joe  Kovalchik  developed  the  theory 
of  convex  polygonal  K-regions  in  his  dissertation  [7].  We  will  adopt  this  theory  to  carry  out 
the  global  path  planning  for  the  solution  to  the  top  layer  of  entire  layered  motion  planning. 
This  section  reviews  the  theory  briefly  and  leave  the  details  to  [7]. 

1.  K-decomposition 

The  K-decomposition  produces  convex  regions  which  are  easier  to  handle  in  the 
local  motion  planner.  The  global  plan,  called  the  path  class,  is  a  sequence  of  convex  K- 
regions,  which  in  turn  specifies  a  sequence  of  borders  belonging  to  these  K-regions  which 
must  be  crossed  in  order  to  execute  the  global  plan. 

a.  Borders 

Consider  the  problem  of  symbolically  representing  each  homotopy  class.  A 
method  based  on  “borders”  is  presented.  (See  Figure  2.3)  A  border  in  a  world  is  a  closed 
line  segment  B  which  satisfies  the  following  conditions: 

(i)  Both  endpoints  are  on  the  world’s  physical  boundary,  dW ,  and 

(ii)  The  open  segment  of  S  is  a  subset  of  Free  (W)  . 

b.  Decompositions  and  Regions 

In  a  world,  a  finite  set 

L=  (Eq2.2) 

of  borders  in  the  world  in  which  two  borders’  open  segments  do  not  intersect  is  called  a 
decomposition  (of  the  free  space  Free(W)  ).  Thus  L  divides  FreeiW)  into  a  finite 
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number  of  regions.  A  region,  R ,  includes  borders  to  which  it  abuts  as  part  of  its  boundaries. 
Therefore,  a  border  belongs  to  at  most  two  regions. 


Figure  2.3:  Borders 


c.  Normal  Path  and  Crossing  Sequence 

A  normal  path  is  a  path  which  does  not  travel  along  a  border,  osculate  a 
border  or  turn  back  when  crossing  a  border.  The  sequence  of  borders  Bj ,  for  i  = 

is  called  the  border  sequence.  The  sequence  of  regions  ,  for  /  =  0, . . .,  ^ ,  is  called  the 

region  sequence.  For  a  normal  path,  f,  the  following  sequence,  |3(/)  ,  is  called  the 
crossing  sequence. 

(i)  (k>0) .  where 

(ii)  the  start  configuration  is  located  in  R^^  = 

(iii)  the  goal  configuration  is  located  in  R^^  -  R^,  and 
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d.  Convex  K-decomposition 


Let  R{W,  L)  be  defined  as  the  set  of  all  regions  contained  in  Free  (W) 

created  by  a  decomposition  £  in  W.  When  £.  is  a  decomposition  of  Free  (W)  such  that 
every  region  of  R(W,  L)  is  convex,  it  is  called  a  K-decomposition.  (see  Figure  2.4) 


Figure  2.4:  The  Example  of  K-decomposition 


Obviously,  there  is  more  than  one  K-decomposition  for  a  given  W.  A 
convex  region  so  created  by  the  above  decomposition  is  called  a  Convex  K-region  and 
satisfies  the  following  conditions;  i)  the  interior  of  the  convex  polygon  lies  completely  in 
free  space,  ii)  each  vertex  defining  the  convex  polygon  is  contained  in  a  physical  boundary 
of  W. 

2.  Global  Path  Planning 

Given  a  world  W,  start  configuration  q^.,  and  goal  configuration  the  global  path 
planning  problem  is  to  find  an  optimal  path  connecting  the  K-region  containing  q^  with  the 
K-region  containing  q^.  This  planning  is  done  by  first  decomposing  Free{W)  into  K- 


regions,  then  building  a  connectivity  graph,  and  finally  using  a  modified  Dijkstra’s 
algorithm  to  search  for  an  optimal  path  represented  by  a  crossing  sequence.  The  K- 
decomposition  is  introduced  as  described  in  the  previous  subsection.  In  this  subsection  we 
review  how  the  optimal  path  is  obtained  after  K-decomposition  of  the  world  is  done. 

a.  Connectivity  Graph 

Given  a  K-decomposition  of  a  world,  its  geometric  adjacency  relationship 
is  represented  by  a  directed  connectivity  graph,  g .  Each  K-region  is  considered  as  a  node 
of  g,  and  each  border  is  considered  as  an  edge  in  g.  Figure  2.5  is  a  decomposed  world 
model  with  border  and  region  names  assigned.  The  connectivity  graph  corresponding  to 
this  world  model  is  shown  in  Figure  2.6.  and  2.7 


Figure  2.5:  A  K-decomposition  of  World  Mode! 


Figure  2.6:  The  Connectivity  Graph  of  a  K-decomposition  of  World  Mode! 


Figure  2.7:  A  Connectivity  Graph 


b.  Path  Class  Representation 


Consider  the  problem  of  finding  a  path  fromi  a  start  configuration  to  a 

goal  configuration  q„  in  a  K-decomposed  world.  In  its  most  general  form,  a  path  class,  P , 
☆ 

is  represented  by  the  crossing  sequence.  In  the  example  of  Figure  2.8  and  2.9,  one  of  the 
path  classes,  which  connect  region  R,  with  region  is  represented  by  a  crossing 
sequence  as  follows; 

P  =  55,  S7,  Sg,  i?g)  (Eq  2.3) 


Figure  2.8:  A  Path  Class  in  the  K-decomposed  Regions 


c.  Finding  Optimal  Path  Class 

A  modified  Dijkstra's  algorithm  is  used  to  find  an  optimal  path  class  given 
two  configurations,  and  q^ .  The  path  class  will  be  found  in  terms  of  the  crossing 
sequences.  Both  position  and  orientation  of  the  start  and  final  configurations  are  taken  into 


consideration  when  performing  the  search  for  the  minimum  cost  path  class.  Relaxation  of 
edges  involving  the  start  and  the  goal  regions  determines  a  cost  by  using  a  bidirectional 
motion-planning  algorithm.  This  algorithm  results  in  achieving  a  better  approximation  of 
the  total  cost  of  the  path  class  by  considering  the  maneuvers  conducted  in  the  initial  and 
final  motions. 


Figure  2.9:  Highlight  of  the  Path  Class  in  Figure  2.8 
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III.  LOCAL  MOTION  PLANNING  APPROACH 


The  regions  on  the  global  path  class  provide  information  for  rough  robot  navigation. 
A  safe  motion  plan  for  accomphshing  the  mission  will  not  be  ensured  without  further 
elaborative  planning.  The  task  of  local  motion  planning  is  to  produce  a  smooth,  collision- 
free  motion  for  the  robot  based  on  the  global  path  class  generated  by  global  path  planner. 
This  chapter  addresses  an  approach  to  the  local  motion  planning.  This  approach  provides 
the  fundamental  concepts  to  be  used  in  the  local  motion  planning  of  this  dissertation.  In 
Section  A,  we  state  the  local  motion  planning  problem.  Section  B  describes  the  preliminary 
planning  decision.  In  Section  C,  we  will  introduce  a  solution  to  the  mobile  robot  motion 
control  “  steering  function.  Section  D  discusses  one  of  planning  methods  —  forerunner 
simulation.  In  Section  E,  the  concepts  of  symmetric  path  and  reverse  path  are  presented. 
Section  F  describes  the  local  motion  planning  steps.  The  planning  details  will  be  further 
discussed  in  Chapter  IV  and  Chapter  V. 

A.  PROBLEM  STATEMENT 

The  robot’s  motion  is  said  to  be  safe  if  its  trajectory  is  collision-free.  The  motion 
planning  starting  from  configuration  qj  to  configuration  ^2  is  said  to  be  symmetric  if  the 
trajectory  of  the  planned  motion  is  exactly  the  same  as  the  trajectory  of  the  motion  planned 
reversely  (from  q2  to  qj  with  reverse  orientations).  The  local  motion  planning  is  as  follows: 

Given  a  K-decomposed  world  model  W,  a  start  configuration  q^,  a  goal  configuration 
q„,  and  a  global  path  class  11  =  ...B- R^),k>0 ,  in  which  q, 

locates  in  region  R^^  and  q^  in  region  R^^ .  The  problem  of  local  motion  planning  is 
that  of  planning  a  safe  motion  symmetrically  for  a  rigid  body  robot  to  travel  from  q^  to 
qg  with  the  global  path  class. 

For  example,  a  given  world  is  decomposed  into  K-regions  as  Figure  3.1.  The  start 
configuration  q^  resides  in  i?;,  and  goal  configuration  q^  in  Rg.  The  given  global  path  class 
is  n  =  </?^,  B5,  R^,  Bg,  Rg,  B7,  Ry,  Bg,  Rg>,  where  the  regions  (covered  by  light  shaded  areas) 
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and  borders  between  them  are  as  depicted  in  Figure  3.1.  The  task  for  local  motion  planner 
is  to  plan  a  safe  motion  for  the  robot.  Its  outputs  will  be  a  safe  and  symmetric  motion  plan. 
The  detail  will  be  discussed  in  Chapter  IV  and  V. 


Figure  3.1:  The  Example  of  K-decom position 


B.  PLANNING  APPROACHES 

1.  Break  Down  Planning 

The  global  path  class  is  the  input  to  local  motion  planning.  It  provides  useful 
information  in  directing  the  robot  to  accomplish  its  mission.  That  information  is  contained 
in  K-regions  separated  by  borders.  It  is  convenient  to  break  down  the  entire  motion 
planning  into  individual  regions.  Therefore,  the  first  decision  of  local  motion  planning  is 
made  to  carry  out  the  planning  region-by-region  along  the  global  path  class. 

With  nonholonomic  and  kinematic  constraints,  the  robot’s  motions  near  the  start 
configuration  and  goal  configuration  should  be  planned  separately  in  order  to  achieve  the 
entire  motion-planning  task.  Therefore,  the  local  motion  planning  is  further  subdivided  into 
two  types  of  motion  planning: 


®  end-portion  motion  planning  and 
®  mid-portion  motion  planning 

The  end-portion  motion  planning  deals  with  the  motion  taking  place  in  the 
regions  near  two  ends  of  the  global  path  class  as  shown  in  Figure  3.2,  and  the  mid-portion 
motion  planning  plans  the  robot’s  motion  exclusively  between  the  regions  near  the  two 
ends  as  illustrated  in  Figure  3.3.  As  stated  in  Chapter  II,  the  initial  region  is  the  first  region 
on  the  global  path  class,  in  which  the  start  configuration  resides.  The  final  region  is  the  last 
region  on  the  global  path  class,  in  which  the  goal  configuration  falls.  Ideally,  the  end- 
portion  motion  planning  involves  only  initial  region  and  final  region.  However,  in  some 
cases,  the  regions  next  to  the  initial  region  and  final  region  may  be  included  in  end-portion 
motion  planning.  The  end-portion  motion  planning  which  involves  the  initial  region  is 
called  initial  motion  planning,  and  the  planning  which  involves  the  final  region  is  called 
final  motion  planning.  This  will  be  discussed  in  Chapter  V. 


Figure  3.3:  The  Mid-portion  Motion  Planning 
2.  Crossing  Border  with  Orthogonal  Orientation 

Under  the  assumption  in  Chapter  I,  obstacles  are  assumed  to  be  rectilinear 
polygons.  Thus,  when  the  free  space  is  decomposed  into  K-regions,  it  is  natural  that  the  K- 
regions  inherit  rectilinear  features  even  though  there  are  a  few  exceptions.  Since  the 
orientation  of  borders  is  orthogonal  in  most  cases,  planning  a  motion  that  crosses  a  border 
at  the  center  or  at  the  point  with  minimum  clearance  from  objects,  with  orthogonal 
orientation  will  be  considered  safe.  This  will  be  the  essential  idea  of  local  motion  planning 
in  the  dissertation.  With  a  predetermined  crossing  point,  the  border’s  orthogonal 
orientation  can  define  a  border  configuration.  Two  such  configurations  on  the  distinct 
border  of  a  region  can  be  taken  to  plan  a  safe  motion  in  the  region.  Thus,  our  primary 
strategy  for  local  motion  planning  is  to  cross  borders  at  crossing  points  with  orthogonal 
orientation  (see  Figure  3.4).  Using  border  configurations  in  local  motion  planning  links 
end-portion  and  mid-portion  motion  planning  together  into  a  complete  motion  plan.  When 
the  entire  motion  plan  is  finished,  it  will  be  as  shown  in  Figure  3.5. 
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Figure  3.4:  Crossing  Borders  with  Orthogonal  Orientations 


Figure  3.5:  A  Complete  Motion  Plan 


C.  TERMINOLOGY 


A  few  other  terminologies  and  concepts  which  are  useful  for  the  discussing  the  local 
motion  planning  are  defined  in  this  section.  A  K-region  is  a  convex  polygon  [7].  The 
boundary  of  a  K-region  consists  of  a  set  of  hne  segments.  Each  line  segment  is  an  edge  of 
the  K-region.  An  edge  of  a  K-region  can  be  a  border,  a  portion  of  physical  boundary  or  the 
combination  of  these  two.  The  region  in  which  the  start  configuration  falls  is  called  the 
initial  region.  The  region  in  which  the  goal  configuration  falls  is  called  the  final  region. 
DEFINITION  3.1  If  a  border  is  one  of  the  edges  of  a  K-region,  the  border  is  called  a 
Full-border  or  F -border.  Otherwise  it  is  called  a  Partial-border  or  P-border. 

DEFINITION  3.2  Let  C  -  <  Rj,  ...,  be  region  sequence  of  a  global  path  class, 
where  n>  2.  For  any  region  Ri,  2  <i  <  n,  there  must  exist  one  and  only  one  border 
between  region  Ri.j  and  Ri.  This  border,  denoted  as  Bij^,  is  called  the  entrance  border 
or  Entrance  to  the  region  R^.  Similarly,  the  border  between  region  R^  and  Ri+j,  1  <i< 
n  is  called  the  exit  border  or  Exit  to  the  region  Ri,  denoted  as 

DEFINITION  3.3  Let  vj=(xj,  yj),  V2=(X2,  y2)  be  two  end  points  of  a  border  in  a  K- 
region.  The  orientation  of  the  border  is  defined  as  an  orientation  which  is  perpendicu¬ 
lar  to  arctan2(y]-  y2,  Xj- X2).  The  orientation  of  the  entrance  border  is  denoted  by 
and  the  orientation  of  the  exit  border  is  denoted  by  . 

For  a  specific  border  (either  the  entrance  border  or  the  exit  border  in  a  region  of  a 
path),  the  orientation  of  the  border  is  decided  by  the  direction  of  crossing  border  motion. 
For  instance,  let  Vj—(xj,  yj),  V2=(X2,  y2)  be  two  end  points  of  the  entrance  border.  If  is 
on  the  left  side  of  the  entrance  direction,  then  the  orientation  of  the  border  is  computed  as 
'P  =  arctan2(yj-  y2,  X/-  X2)  -  Tt/2.  Figure  3.6  and  Figure  3.7  illustrate  these  concepts  in 
DEHNITION  31,  DEFINITION  3.2  and  DEFINITION  3.3. 
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vi=(xi,yi)  I 
I 
I 


P-Border 


I  ^in  =  arctan2(yj- y2,  Xj- X2)  -  n/2 


F-Border  I 
V2=(X2,  y2)  * 


out 


i+1 


I 


Figure  3.6:  Full-Border,  Partial-Border  and  Orientation  of  Border. 


For  a  region  in  a  global  path  class,  the  edge  that  contains  the  entrance  border  is 
called  the  Entrance  Edge,  denoted  by  Ej„,  and  the  edge  that  contains  the  exit  border  is 
called  the  Exit  Edge,  noted  by  We  also  denote  the  center  of  an  entrance  edge  as  EQn, 
the  center  of  an  exit  edge  as  the  center  of  an  entrance  border  as  BQ^,  and  the  center 
of  an  exit  border  as  For  any  rectangular  region,  there  are  two  pairs  of  edges.  The  pair 

of  edges  that  are  parallel  to  the  orientation  of  the  entrance  border  is  called  Forward  Edge, 
denoted  by  FE.  The  length  of  Forward  Edge  is  called  Forward  Length  of  the  region, 
denoted  by  FL.  The  pair  of  edges  which  is  perpendicular  to  the  entrance  border  is  called 
Cross  Edge,  denoted  by  CE.  The  length  of  Cross  Edge  is  called  Cross  Length,  denoted  by 
CL.  Figure  3.7  shows  the  definitions.  A  configuration  defined  by  a  point  Pi^  on  the 
entrance  border,  the  orientation  and  a  curvature  (normally  zero)  is  called  an 
entrance  configuration,  denoted  by  =  (Pin,  ^j„).  A  configuration  defined  by  a 
point  Pont  on  the  exit  border,  the  orientation  a  curvature  (normally  zero)  is 

called  an  exit  configuration,  denoted  by  q^ut  =  (Pout’  ^out  KuO-  When  the  curvature  is 
set  to  0,  the  entrance  configuration  and  exit  configuration  specify  a  straight  line  that  passes 
the  entrance  and  exit  border  respectively. 


^i-1 

CL 


Figure  3.7:  Entrance  Border,  Exit  Border,  Entrance  Edge,  Exit 
Edge  and  their  Centers. 

Basically,  there  are  three  major  ways  for  entrance  and  exit  borders  to  be  positioned 
on  the  edges  of  a  K-region.  We  categorizes  intra-region  motions  in  a  K-region  into 
following  three  types: 

♦  Type  1:  The  entrance  and  exit  borders  are  parallel.  (Figure  3.8) 

♦  Type  n:  The  entrance  and  exit  borders  are  on  edges  which  share  the  same  vertex 
of  a  K-region.  (As  a  special  case,  the  entrance  and  exit  share  the  same  vertex  of 
the  K-region.)  (Figure  3.9) 

♦  Type  III:  The  entrance  and  exit  borders  are  on  the  same  edge  of  the  K-region 
(Figure  3.10) 


Figure  3.8:  An  Intra-region  Motion  of  Type  I 


Entrance 


t 

Exit 


Figure  3.9:  An  Intra-region  Motion  of  Type  II 


Entrance  Exit 

Figure  3.10:  An  Intra-region  Motion  of  Type  in 


D.  MOTION  PLANNING  WITH  STEERING  FUNCTION 
1.  Smooth  Motion  Planning  with  Curvature 

An  ordinary  non-holonomic  ground  robot  vehicle  has  two  control  variables;  the 
curvature  k  of  the  motion  trajectory  and  the  linear  speed  v.  Since  a  non-holonomic  robot’s 
heading  orientation  0  is  always  equal  to  the  trajectory’s  tangent  orientation,  the  vehicle’s 
rotational  speed  O)  is  equal  to  (because  oi  =  dQ  I  dt  =  (dQ  I  ds)  (ds/dt)  =  kv,  where  t  is 
time  and  s  is  the  traveling  length  of  the  robot).  Therefore,  the  smooth  motion  planning  for 
a  mobile  robot  is  to  design  (k,  v)  or  (o),  u)  as  function  of  t  or  v. 
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This  control  model  with  curvature  is  useful  for  vehicles  with  any  kinematics  [18]. 
If  a  vehicle  has  differential  drive  type  kinematics,  with  a  tread  of  2W,  its  right  wheel  speed 
v+  and  left  wheel  speed  v_  should  be  v^.  =  (1  ±  W  Ic)  u  =  u  ±  Wo).  If  a  vehicle  has  bicycle 
type  kinematics  with  a  distance  L  between  two  wheels,  the  orientation  (|)  of  the  front 
steering  wheel  should  be  0  =  arctan(L  k).  Thus  the  real  time  evaluation  of  (k,  v)  appears  to 
be  the  most  direct  method  in  smooth  vehicle  control. 

One  obvious  method  is  to  compute  the  curvature  directly  as  a  function  of  the 
geometrical  constraints  and  the  mission.  However,  one  drawback  of  this  method  is  that 
when  some  of  the  input  has  a  discontinuity  from  the  previous  value,  the  output  k  also  tends 
to  be  discontinuous.  As  widely  known,  a  rigid  body  motion  with  a  discontinuous  curvature 
function  is  not  physically  realizable.  In  order  to  solve  this  problem,  we  compute  the 
derivative  of  the  curvature  instead  of  the  curvature  itself. 

2.  Steering  Function 

The  best  method  for  smooth  vehicle  navigation  known  so  far  by  us  is  to  compute 
the  derivative  of  curvature,  dk  /  ds,  as  a  function  of  the  geometric  information  and  the 
mission  (This  function  is  called  steering  function).  After  computing  this  value  dk  /  ds  =/, 
the  curvature  k  is  updated  through  the  incremental  movement  As.  As  long  as /is  a  finite 
value,  this  method  always  gives  a  smooth  trajectory  in  any  circumstance.  In  the 
mathematical  model,  we  understand  the  vehicle’s  curvature  is  not  rapidly  changed,  hence 
we  include  in  the  vehicle’s  configuration.  A  configuration  q  is  a  triple  (p,  0,  k)  of  position, 
orientation,  and  cxuvature.  We  have  found  the  following  steering  function  works  in  all 
situations  we  have  applied: 

dk 

—  =  -(AA/:  +  fiAe  +  CAd)  (Eq3.1) 

In  Eq  3.1,  A,  B,  and  C  are  positive  constants  which  are  related  to  the  smoothness  of 
robot’s  motion  [19].  The  meanings  of  these  variables,  Ak,  A6,  and  Ad,  are  as  follows:  Afc 
is  the  difference  between  the  current  vehicle’s  curvature  k  and  the  desired  curvature  k^.  A0 


30 


is  the  difference  between  the  current  vehicle’s  orientation  0  and  the  desired  orientation 
Ad  is  the  vehicle’s  position  error  (The  exact  equation  is  determined  by  situations.  For 
instance,  if  the  robot  is  tracking  a  directed  reference  path,  it  is  the  “signed”  distance  from 
the  vehicle  position  to  the  directed  path). 

All  geometric  information  obtained  by  a  robot  must  eventually  be  converted  into  a 
triple  r  =  (Afc,  A0,  Ad)  of  the  curvature  difference,  orientation  difference,  and  position 
error.  In  most  geometric  situations,  this  interpretation  task  is  surprisingly  straightforward. 
Once  this  information  F  is  obtained,  the  robot’s  motion  can  be  controlled  as  desired. 

3.  Line  Tracking  with  Steering  Function 

a.  Introduction 

The  steering  function  smooths  the  line  tracking  motion.  In  the  line  tracking 
motion,  the  configuration  where  the  tracking  motion  starts  is  called  start  configuration, 
denoted  by  =(Ps,Q  g,  kg).  The  line  that  is  the  goal  of  line  tracking  is  called  the  reference 
line,  denoted  by  =(pg,Qg,kg).  We  realize  that  a  straight  line  has  zero  curvature.  In  this 
dissertation  we  focus  on  straight  line  tracking.  Thus,  the  third  element  in  the  configuration 
which  specifies  a  line  is  normally  set  to  zero.  The  closest  distance  [19]  from  the  point 
Pg,wheTe  the  tracking  motion  starts,  to  the  reference  line  is  called  the  initial  vertical 
distance,  denoted  by  The  length  of  the  reference  line  from  the  point,  which  is  defined 
by  the  image  of  the  starting  point  Pg  on  the  line,  to  the  point,  where  the  tracking  trajectory 
converges  to  the  line,  is  called  convergence  length,  denoted  by  L.  Figure  3.11  illustrates 
an  example  of  line  tracking  using  steering  function.  The  line  tracking  can  be  performed  by 
robot  either  forward  or  backward.  In  forward  line  tracking  the  robot  advances  forward  each 
step  in  the  sense  of  robot’s  heading  orientation.  This  is  the  most  common  use  of  line 
tracking.  This  section  describes  the  general  idea  of  line  tracking.  The  detailed  tracking 
technique  was  described  as  path  tracking  in  [20]  [21]. 
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Figure  3.11:  The  Example  of  Line  Tracking 
b.  T  wo  Important  Types  of  Line  Tracking 

Line  tracking  can  be  performed  between  an  arbitrary  start  configuration  and 
a  reference  line.  We  are  interested  in  two  special  pairs  of  start  configuration  and  reference 
line.  They  are  considered  as  two  basic  types  of  line  tracking.  The  first  type  is  parallel  line 
tracking,  in  which  the  orientation  of  the  start  configuration  is  equal  to  that  of  the  reference 
line  as  in  Figure  3.12.  The  second  type  is  perpendicular  line  tracking,  in  which  the 
orientation  of  the  start  configuration  is  perpendicular  to  the  orientation  of  the  reference  line, 
as  Figure  3.13.  If  the  task  requires  the  tracking  motion  converge  to  the  reference  line  in  a 
K-region,  the  convergence  length  is  limited.  The  limited  convergence  length  is  denoted  as 
Tallowed  t)Oth  Figure  3.12  and  Figure  3.13.  For  parallel  line  tracking,  the  minimum  length 
of  convergence  is  LaUowed  -  ^-02  (see  Eq  4.9  and  the  analysis  in  Chapter  IV).  For 
perpendicular  line  tracking  the  minimum  length  of  convergence  is  -  3-38  (see 

Eq  4.10). 
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Figure  3.12:  The  Example  of  Parallel  Line  Tracking  in  K-region. 


Qs  (Ps’^  S’ 


Figure  3.13:  The  Example  of  Perpendicular  Line  Tracking  in  K-region. 

E.  FORERUNNER  SIMULATION 
1.  Overview 

A  forerunner  is  a  virtual  robot  which  is  used  to  simulate  a  real  robot’s  behavior.  A 
simulation  that  uses  the  foreruner(s)  to  simulate  the  robot’s  motion  in  path  tracking  is  called 
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Forerunner  Simulation.  Forerunner  Simulation  can  be  applied  to  motion  planning  and 
motion  control  in  many  aspects.  For  instance,  it  can  be  used  to  determine  the  leaving  point 
in  the  line-to-line  transitioning.  Using  Forerunner  Simulation  in  transition  point 
computation  is  very  meaningful  in  real-time  robot  motion  control.  We  will  describe  this 
application  in  Chapter  VII.  Forerunner  Simulation  is  especially  helpful  in  local  motion 
planning.  In  this  section  the  mechanism  of  constructing  a  Forerunner  Simulation  will  be 
discussed. 

As  we  know,  Forerunner  Simulation  uses  a  virtual  robot  to  simulate  a  real  robot’s 
motion.  Thus  it  must  perform  all  computations  that  are  taken  in  a  real-time  robot  control 
system,  except  the  portion  that  is  tightly  related  to  the  robot’s  hardware.  In  this  dissertation, 
the  real-time  robot  control  system  refers  to  the  Model-based  Mobile  robot  Language 
(MML)  which  will  be  introduced  in  Chapter  Vn,  Vin,  and  Chapter  IX. 

2.  Forerunner  Simulation  Structure 

The  steering  function  described  in  Section  A  of  this  chapter  and  the  Appendix  A  is 
fundamental  component  in  MML,  and  is  naturally  inherited  to  the  Forerunner  Simulation. 
The  framework  of  motion  control  is  the  use  of  a  steering  function  to  compute  the  required 
curvature  change  in  each  motion  cycle.  The  curvature  change  is  in  turn  used  to  compute  the 
forerunner’s  new  position.  Given  an  initial  configuration  =  ((xi^ip  yim),  6j„f,  kif^)  as  the 
forerunner’s  current  configuration  q  =  ((x,  y),  0,  k)  and  a  reference  path  of  configuration 
path  =  ( (Xp,  yp),  Qp,  kpj.Based  on  this  frame  work,  the  Forerunner  Simulation  is  constructed 
by  computing  the  following  elements: 

•  Constants  of  Steering  Function 

»  Curvature  Change 

•  New  Curvature  and  Orientation  Change 

»  New  Configuration  of  Next  Step 
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a.  Constants  of  the  Steering  F unction 

The  constants  A,  B,  C  of  the  steering  function  are  determined  by  the  motion 
smoothness  a  as  follows: 


k  =  ]  1  o 

(Eq3.2) 

A  =  3k 

(Eq  3.3) 

(Eq  3.4) 

C  =  k? 

(Eq3.5) 

Because  the  smoothness  is  a  variable,  it  be  can  changed  at  any  time.  Especially  when 
dynamic  smoothness  strategy  is  applied,  the  smoothness  is  determined  by  the  environment, 
for  instance  by  the  size  of  the  K-region.  Thus  the  constants  A,  B  and  C  have  to  be  computed 
whenever  the  smoothness  o  is  changed. 

b.  Curvature  Change 

For  the  current  configuration  q  =  ((x,  y),  9,  k),  the  reference  path 
configuration  path  =  ((Xp,  yp),  Qp,  kp),  and  constants  A,  B,  and  C,  we  compute  the  curvature 
change  as  follows: 

Ad=(y-  yp)  *  cos(Qp)  -  (x  -  Xp)  *  sin(dp)  (Eq  3.6) 

dk/ds  =  -(A(k-  kp)  +  B  (9  -  ep  +  C  Ad)  (Eq  3.7) 

c.  New  Curvature  and  Orientation  Change 

Once  the  curvature  change  dk  /  ds  has  been  computed,  we  obtain  the  new 
curvature  and  orientation  change  by  following  calculations: 

knew  =  k  (dk !  ds)  *  As  (Eq3.8) 

AQ^knew*^  (Eq3.9) 

here  As  is  the  transitional  length  the  foremnner  advances  in  each  sampling  step  and  this 
value  is  given  by  the  forerunner  designer.  Normally  As  is  positive  for  forward  forerunner 
to  increment  it  next  position  forward.  The  value  A9  represents  the  estimated  orientation 
change  between  the  current  position  and  next  position. 
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d.  New  Configuration  of  Next  Step 

Given  the  incremental  transitional  length  As  and  the  estimated  orientation 
change  A6,  we  can  compute  the  relative  configuration  q^.  =  ((x^,  y^.),  6^,  kf.)  (with  respect  to 
the  local  coordinate  system  of  the  current  configuration  q  =  ((x,  y),  0,  k))  of  the  new 
configuration  in  next  sampling  step  where  [19]: 


Xr=  (1  -  A0^  /  6)  *  As 

(Eq3.10) 

y,^(l  -  AG^  / 12)  *  (Ae  /  2)  *  As 

(Eq3.11) 

CD 

11 

> 

CD 

(Eq3.12) 

II 

CD 

O 

(Eq3.13) 

The  new  configuration  =  ((x^g^f  ynext)’  ^nexf  ^next)  of  the  next  sampling  step  is  then 
computed  by  composing  the  current  configuration  q  and  relative  configuration  q^  as 
follows: 


Xfiext  =  -^  +  cos(0)  *  Xf-  sin(Q)  *  y^ 

(Eq3.14) 

ynext  =  >’  +  sin(B)  *  Xr  -  cos(0)  * 

(Eq3.15) 

^next  =  0  +  0r 

(Eq3.16) 

k  -Jr 

^next  ^new 

(Eq3.17) 

3.  Algorithms 

With  these  elements,  a  forerunner  simulation  template  is  constructed  by  the 
following  algorithms  as  shown  in  Figure  3.14  and  Figure  3.15. 

As  previously  mentioned,  the  Forerunner  Simulation  can  be  applied  to  many 
different  applications.  When  the  simulation  is  developed  for  an  application  with  a  specific 
purpose,  the  code  for  performing  the  application  and  related  preWORK  and  postWORK 
will  be  inserted  into  the  template.  The  condition  for  the  while  loop  and  desired  return  value 
will  be  specified  also. 
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Algorithm  FRsimulation  (q,  p,  a,  others) 

Input:  q:  an  initial  configuration;  p:  a  reference  path;  a:  a  smoothness; 

others:  other  parameters  if  any; 

Output:  depending  on  application 

(1)  Perform  preWORK; 

(^)  dnew  ~  d’ 

(3)  Compute  constants  of  steering  function; 

(4)  while  ( condition  not  matched ) 

dnew  =  AdvanceForerunner  ( q^^^  p); 

(6)  Application  codes; 

(7)  end  while; 

(8)  Perform  postWORK; 

(9)  return  ( desired  value); 


Figure  3.14:  The  Algorithm  for  Forerunner  Simulation  Template. 


Algorithm  AdvanceF orerunner  (q,p) 

Input:  q:  the  current  configuration;  p:  the  reference  path; 
Output:  next  configuration 

(1)  Compute  curvature  change  in  forward  direction; 

(2 )  Compute  new  curvature  and  orientation  change; 

(3)  Compute  new  configuration  of  next  step ; 

(4)  return  ( new  configuration ) 


Figure  3.15:  The  Algorithm  for  Advancing  Forerunner  in  One  Step. 


F.  SYMMETRIC  MOTION  PLANNING  AND  REVERSE  PATH 

For  a  configuration  q  =  (p,  0,  k),  its  reverse  configuration  rev(q)  is  defined  by  a 
tuple  as  (p,  7C  +0,  -  k).  Let  qj  =  (pj,  Q],kj)  and  ^2  =  fP2>  ®2'  ^2)  be  two  distinct  configurations 
on  the  world.  A  motion  planning  method  is  said  to  be  symmetric  if,  for  any  configuration 
qj  an  q2,  the  trajectory  of  motions  planned  from  to  <72  is  exactly  the  same  as  the  trajectory 
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of  motions  planned  from  rev(q2)  to  rev(qi ).  Figure  3.16  illustrates  an  example  of  symmetric 
motion  planning  corresponding  to  Figure  3.5. 


Figure  3.16:  A  Symmetric  Motion  Plan 


The  steering  function  is  powerful  tool  for  producing  a  smooth  trajectory  while 
tracking  a  path.  For  smoothness  considerations,  when  using  the  steering  function  to  track  a 
path,  the  trajectory  moves  toward  the  reference  path  quickly  at  the  beginning,  but  it  slows 
down  when  the  trajectory  is  getting  closer  to  the  path.  Thus  a  symmetric  motion  planning 
cannot  be  done  by  merely  applying  path  tracking  to  reverse  configurations  with  the  steering 
function. 

For  example,  in  Figure  3.17,  the  start  configuration  is  q^  =  (Ps,  ^s>  ^s)-  Tb®  is 
to  track  the  line  specified  by  qg  =  (pg,  6^,  kg),  finally  stopping  at  or  passing  through  the 
configuration  qg.  The  trajectory  converges  to  the  line  at  the  configuration  q^  =  (p^,  6^,  kj. 
Now  tracking  the  line  specified  by  configurations  rev(q^)  from  the  configuration  rev(qg), 
we  will  have  a  trajectory  as  Figure  3.18.  The  motion  plan  in  Figure  3.17  is  obviously  not 


symmetric  to  that  of  Figure  3.18,  even  though  their  two  end  configurations  might  be  the 


same. 


(Ps’ 


rev(qs) 


rev(qg) 

Figure  3.18:  A  Reverse  Line  Tracking 

The  trajectory  in  Figure  3.17  can  be  produced  in  real  time  by  tracking  a  line 
specified  by  the  configuration  q^  from  the  configuration  q^  But  in  real  time  there  is  no  way 
to  produce  such  a  trajectory  by  simply  applying  line  tracking  from  rev(qg)  to  rev(q^). 
Fortunately,  the  foremnner  simulation  can  be  used  to  solve  the  symmetric  motion  planning 
problem.  As  mentioned,  the  foremnner  is  a  virtual  robot  which  can  be  used  to  simulate  a 
real  robot’s  behavior.  Again  we  assume  q^  and  q^  are  the  start  and  goal  configurations 
respectively.  For  symmetric  motion  planning,  we  set  the  forerunner  at  rev(qJ  to  perform 

o 

path  tracking  with  rev(qj  as  its  reference  line.  The  simulated  trajectory  can  be  stored  as  a 


sequence  of  configurations.  A  reverse  path  can  be  constructed  by  reversing  each 
configuration  and  rearranging  configurations  in  the  sequence  with  opposite  order.  Then  the 
symmetric  motion  planning  involves  generating  and  tracking  reverse  paths.  TTie  reverse 
path  is  formulated  in  the  following  two  steps: 

♦  Step  1.  Set  the  forerunner  to  the  reverse  configuration  of  goal  configuration 
and  run  forerunner  simulation  to  track  a  line  specified  by  reverse  configuration 
of  start  configuration  While  the  forerunner  is  traveling,  store  the  trajectory 
(by  configurations)  until  the  tracking  converges  to  the  reference  line.  We 
represent  this  path  with  a  sequence  of  configurations  as: 

C  =  <qj,...,  qi,...,  qfi>  where  n>l  and  qi  =  (p^,  0,-,  k^)  for  all  1  <i  <n; 

•  Step  2:  Reverse  the  entire  sequence  to  generate  a  reverse  path  as  follows: 

Cfey  —  qi,--,  qj^  (Eq  3.18) 

where  n>l  and  qi  =  (pi,  it  +  0,,  -  ^,)  for  all  1  <i  <n; 

Ideally,  the  first  configuration  q^  in  path  should  specify  a  line  that  is  identical 
to  the  line  specified  by  the  start  configuration  q^  =  (p^,  0^,  k^). 

If  a  local  motion  planning  method  is  symmetric,  reverse  paths  will  be  generated  for 
many  cases.  Circumstances  under  which  a  reverse  path  needs  to  be  generated  will  be 
discussed  in  Chapter  IV  and  V. 

G.  REVERSE  PATH  TRACKING 

The  normal  path  tracking  provides  a  technique  for  vehicle  odometry  correction  to 
catch  up  to  the  reference  path.  The  reference  path  is  normally  specified  by  a  configuration, 
and  the  vehicle  odometry  error  can  be  computed  based  on  that  configuration.  A  reverse  path 
consists  of  a  sequence  of  configurations,  as  shown  in  Eq  3.18.  Each  configuration  in  the 
sequence  can  be  a  reference  path  element  to  the  vehicle  when  the  vehicle  is  tracking  the 
reverse  path.  The  difference  between  two  consecutive  configurations  is  normally  small  in 
all  elements  of  the  configuration.  While  the  vehicle  is  moving,  determining  which 
configuration  in  the  sequence  is  to  be  taken  as  a  reference  path  becomes  critical  issue.  In 
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order  to  track  the  reverse  path  as  precisely  as  possible,  it  is  necessary  to  have  a  new  path 
tracking  technique  other  than  the  normal  one.  This  section  addresses  how  the  reverse  path 
tracking  can  be  performed  properly.  We  begin  by  determining  the  path  tracking 
smoothness. 

Since  the  difference  between  consecutive  configurations  is  small,  the  vehicle’s 
turning  motion  needs  to  be  sharpened  so  that  it  can  converge  to  the  reference  path  faster. 
Therefore,  the  smoothness,  a,  of  the  steering  function  in  reverse  path  tracking  should  be 
smaller  than  the  smoothness,  Gg,  used  to  generate  that  reverse  path.  Our  experiments 
showed  that  the  proper  smoothness  for  reverse  path  tracking  is  as  Eq  3.19; 

o=Og/2  (Eq3.19) 

Let’s  assume  that  a  reverse  path  is  generated  by  forerunner  as  illustrated  in  Figure 
3. 19.  In  the  figure,  the  reverse  path  IT  consists  of  the  configurations  sequence 

(this  is  for  purpose  of  illustration;  the  actual  sequence  will  be  much  denser  to  specify  the 
path  more  precisely),  and  (py,  0^,  ky)  is  the  vehicle’s  current  configuration.  A  global 
coordinate  system  (GCS)  is  defined  as  shown  in  the  figure.  To  track  this  path  11  smoothly 
by  using  the  steering  function,  the  vehicle  takes  the  first  configuration  qj  =  (pj,Qj,  k^)  as 
the  initial  reference  path.  Then  the  differences  of  A0,  M,  and  Ad  are  calculated  as  follows: 

A0  =A0^  -  A0^  (Eq  3.20) 

Ak  =Ak,  -  Akj  (Eq  3.21) 

Ad  =y*  (Eq  3.22) 

The  signed  distance  value  y*  in  Eq  3.22  is  the  shortest  distance  between  the 
vehicle’s  current  configuration  and  reference  path  [20].  These  differences,  A0,  and  Ad, 
are  applied  to  the  steering  function  to  determine  vehicle’s  necessary  curvatiue  in  the  next 
control  cycle  so  that  the  vehicle  is  made  to  travel  along  the  reference  path  smoothly. 
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As  the  vehicle  is  moving  along  the  path  specified  by  a  give  configuration,  a  critical 
decision  must  be  made  to  allow  the  vehicle  to  transition  from  the  current  reference  path  to 
the  next.  There  is  a  simple  way  to  determine  when  to  transition,  as  described  below.  This 
method  uses  the  relative  position  of  the  vehicle’s  current  configuration  and  the 
configuration  that  specifies  the  current  reference  path  element  to  determine  whether  the 
vehicle  has  passed  the  position  of  reference  path.  For  this  purpose,  a  local  coordinate 
system  (LCS)  is  defined  as  illustrated  in  Figure  3.20.  The  LCS  takes  the  center  of  the 
vehicle  as  its  origin.  A  line,  which  passes  the  origin  and  is  parallel  to  vehicle’s  global 
orientation,  defines  the  positive  x  axis  of  the  LCS.  The  positive  y  axis  of  LCS  is  then 
defined  by  a  perpendicular  line  (to  the  x  axis),  passing  through  the  local  origin  and  heading 
to  the  vehicle’s  left  as  shown  in  Figure  3.20. 

When  the  local  coordinate  system  is  established,  the  configuration  of  reference  path 
can  be  Uansformed  from  global  GCS  to  LCS.  The  computation  of  the  configuration 
ttansformation  is  as  following.  Let^^  =(p^,  8^,  k^)  and  (Prep  ^rep  Kef)  the  vehicle’s 
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current  configuration  and  reference  path  configuration  in  GCS  respectively.  Assume  that 
the  reference  path  configuration  ^^^y^with  respect  to  LCS  is  q*  =(p*,  0*,  k*).  Then  we  have 


Qv  0 

(Eq  3.23) 

Qv  ^  Qv^  ^ 

(Eq  3.24) 

> 

o 

II 

* 

(Eq  3.25) 

In  computing  q*,  we  will  simplify  the  configurations  to  transformations  [20].  Thus, 

'T 

the  transformation  q*  =(x*,  y*,  0*j  stands  for  configuration  q*  =(p*,  0*,  k*). 
Transformation  q^  =(Xy,  y^,  %)  stands  for  configuration  q^  =(Py,  0v,  k^)  and  transformation 

'p 

Qref  -(^ref’  yref>  ^ref)  Stands  for  configuration  qref=  (Pref>  ^rep  ^ref)-  Then  we  have 
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Q*  =  q,  = 

^ref 

-x^cosG^-y^sinG^ 
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G  , 
L 

(^ref-^v)  cose^  +  sin0^ 

-ix,ef-x^)  sine^  +  {y,,f-yj  cosG^ 


(Eq  3.26) 


The  signed  valued  x*  =  (x^gf-  cosG^  +  (yref-  Jv)  sinQy  can  be  used  to  determine 
when  to  transition  to  the  path  specified  by  the  next  configuration  in  the  sequence.  When  x* 
becomes  negative,  it  means  that  the  vehicle  has  passed  the  position  of  the  current  reference 
path  segment.  Then  the  next  configuration  is  taken  as  reference  path. 


H.  LOCAL  MOTION  PLANNING  STEPS 

The  local  motion  planning  is  executed  in  the  following  steps: 

•  Compute  intermediate  configurations 

•  Perform  end-portion  motion  planning. 

•  Perform  mid-portion  motion  planning. 

The  first  step  can  be  viewed  as  a  preprocessing  of  local  motion  planning.  The 
purpose  of  preprocessing  is  to  make  local  motion  planning  standardized  and  simplified.  In 
this  section  we  discuss  the  preprocessing  step,  and  an  overall  algorithm  for  local  motion 
planning  is  provided.  We  will  present  the  details  of  end-portion  and  mid-portion  motion 
planning  to  Chapter  IV  and  V. 

The  local  motion  planner  performs  local  motion  planning  region-by-region.  The 
means  of  connecting  the  motion  in  consecutive  regions  becomes  more  important  at  this 
moment.  The  exit  border  of  a  region  is  the  entrance  border  of  next  region  in  the  global  path 
class.  Thus,  a  border  can  serve  as  the  interface  of  two  regions’  motion  communication.  We 
define  an  intermediate  configuration  for  each  border  in  the  crossing  sequence  using  the 
border’s  crossing  point  and  orientation  so  that  the  intermediate  configurations  in  a  region 


44 


can  be  used  to  plan  robot’s  motion  in  that  region  while  connecting  the  motion  to  the  next 
region.  How  to  determine  the  crossing  points  on  borders  will  be  discussed  in  this  section. 
In  order  to  standardize  and  simplify  the  local  motion  planning,  an  orthogonalized  border 
orientation  needs  to  be  computed  for  those  borders  that  are  not  parallel  to  X  or  Y  axis.  After 
the  crossing  point  and  orientation  of  a  border  are  determined,  an  intermediate  configuration 
for  that  border  can  be  defined.  For  example,  if  p  represents  the  crossing  point  obtained  and 
9  represents  the  border  orientation,  then  the  configuration  g  is  defined  as  q  =  (p,  Q,  k), 
where  curvature  k  =  0. 

1.  Determination  of  Crossing  Position  on  the  Border 

Since  the  robot’s  motions  in  neighboring  regions  of  the  path  are  related,  it  is 
necessary  to  consider  the  motion  in  the  region  that  follows  the  current  one.  The  crossing 
point  on  a  border  actually  connects  and  controls  the  motion  on  the  two  regions  that  shares 
the  common  border.  Thus,  determining  where  to  cross  the  border  between  two  regions  is 
important  to  the  local  motion  planning.  A  proper  position  to  cross  the  border  can  make  local 
motion  planning  easier  in  each  region.  In  this  dissertation,  safety  is  the  most  important 
factor  in  local  motion  planning.  Therefore,  the  crossing  point  on  the  border  must  be  in  a 
range  that  allows  the  vehicle  to  cross  the  border  without  collision.  A  border’s  safe  crossing 
range  depends  upon  the  vehicle’s  width  and  the  minimum  clearance  (if  required).  The 
global  path  class  for  motion  planning  is  represented  by  a  sequence  of  all  passable  regions 
accompanied  by  their  related  borders.  This  implies  that  every  border  has  a  safe  range. 
Therefore,  crossing  a  border  at  its  center  (mid-point)  will  be  the  most  desirable  plan  in  the 
sense  of  safety.  We  will  apply  this  idea  in  the  local  motion  planning  in  the  following 
chapters.  However,  there  is  an  exception  in  which  we  do  not  take  the  center  of  the  border 
as  a  crossing  position.  That  is  in  some  special  regions  with  parallel  entrance  and  exit 
borders  and  in  which  following  situations  exist: 

♦  At  least  one  of  the  centers  of  the  entrance  and  exit  borders  has  its  image  (a 
perpendicularly  projected  point)  on  another  border  in  the  safe  range. 
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•  FL<2.02*  d,  where  FL  is  the  forward  length  of  the  region  and  d  is  the  distance 

between  centers  of  the  two  borders  measured  in  the  direction  perpendicular  to 
the  entrance  orientation. 

In  such  a  region,  keeping  the  strategy  of  crossing  the  border  at  its  center  will  make  local 
motion  planning  complicated  and  is  undesirable,  because  for  these  two  parallel 
configurations  there  is  not  enough  length  for  parallel  line  tracking  from  the  entrance 
configuration  to  converge  to  the  exit  configuration  (see  Eq  4.9  in  Chapter  IV  Section  B). 

Figure  3.21  illustrates  a  possible  region  in  a  global  path  class.  In  the  figure,  the 
region,  Rj^,  is  the  region  that  needs  to  be  considered  specially.  The  solution,  which  is  to 
relocate  the  crossing  point  of  the  border  at  the  image  of  another  border’s  center,  is  also 
shown  in  the  figure. 

The  reasons  for  relocating  the  crossing  point  in  this  kind  of  region  are  because 
entrance  and  exit  borders  are  parallel,  and  the  region  has  not  enough  length  in  the  forward 
direction  to  perform  a  complete  parallel  line  tracking  (from  entrance  configuration  to  exit 
configuration  and  converge  in  the  region).  Under  this  situation  the  local  motion  planning 
will  be  forced  to  plan  the  motion  using  other  than  parallel-line  tracking.  Thus,  relocating 
the  crossing  point  on  E-border  makes  two  crossing  points  in  a  straight  line  which  is  exactly 
on  the  exit  configuration.  The  motion  in  this  region  then  will  be  simpler  and  more  desirable. 


Relocated  crossing  point 


Figure  3.21:  Relocating  Border  Crossing  Point  in  an  Exceptional  Region. 
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2.  Orthogonalizing  Border  Orientations 

As  we  realize,  K-region  decomposition  allows  non-orthogonal  borders  in  regions  as 
illustrated  in  Figure  3.1.  In  Figure  3.1,  the  region  and  share  a  non-orthogonal  border 
65.  The  orientation  of  border  Bg  can  be  orthogonalized  to  its  nearest  orthogonal  orientation 
as  illustrated  in  Figure  3.22.  Orthogonalizing  the  orientations  of  non-orthogonal  borders 
makes  the  region  standardized  in  the  sense  of  orthogonal  entrance  and  exit  borders.  The 
local  motion  planning  then  can  be  performed  by  simply  using  parallel  line  tracking  and 
perpendicular  line  tracking  techniques. 


3.  Algorithm 

Naturally,  the  local  motion  planning  is  conducted  in  the  following  order; 

•  Compute  intermediate  configurations 

•  Perform  initial  motion  planning. 

•  Perform  mid-portion  motion  planning. 

•  Perform  final  motion  planning. 
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This  is  under  the  assumption  that  all  end-portion  motion  planning  can  be  done  in 
initial  and  final  regions.  Under  this  assumption,  the  mid-portion  motion  planning  is  carried 
out  region-by-region  from  the  second  region  of  the  global  path  class  to  the  region  next  to 
the  final  region  on  the  path.  The  following  algorithm  for  local  motion  planning  is  based  on 
this  assumption.  If  the  final  motion  planning  involves  more  than  one  region,  the  order  of 
planning  needs  to  be  modified  to  let  the  end-motion  planning  be  finished  before  mid¬ 
portion  motion  planning  starts. 


Algorithm  LocalMP  (q^,qg,U,W) 

Input:  q^  start  configuration; 
q^:  goal  configuration; 

IT;  a  crossing  sequence; 

W;  a  world  model 

Output:  Motion  planning  data  structure 

(1)  if  (length  (Tl}  =  1)  then 

(2)  SingleRegionMP  (q^,  q^,  W); 

(3)  else 

(4)  Q  =  QQ  =  ComputeIntConfig(U,W); 

(3)  Q  =  Qs; 

(6)  MP  =  null; 

qgxit=  car(Q); 

(8)  ^current  ~  car(Y\), 

(9)  append  (MP,  InitialMP  (q,  q^xU’  ^current’ 

(10)  q  —  qgxit’ 

(11)  n  =  cdr  ( cdr  (Yl)); 

(12)  Q  =  cdr(Q); 

(13)  ^current  ~  car(Y\), 

(14)  while  (not  empty( Q)) 

(15)  Qexit^  car(Q); 

(16)  append  (MP,  MidPortionMP  (q,  W); 

(17)  q  =  qexit: 

(18)  n  =  cdr  ( cdr  (U)); 

(19)  Q  =  cdr(Q); 

(70)  rc^,xent=  car(n); 

(21)  end  while  ; 

(22)  append  (MP,  FinalMP  (q,  q^,  ^))’ 

(23)  end  if; 
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The  subroutine  SingleRegionMP  is  a  motion  planning  subroutine  for  a  global  path 
class  having  only  one  region,  i.e.  the  start  and  goal  configurations  reside  in  the  same  region. 
Subroutine  ComputeIntConflg  includes  two  steps.  First,  it  determines  the  crossing  point  of 
each  border  by  the  rule  described  earlier  in  this  section.  Second,  it  orthogonalizes  the 
orientations  of  borders.  This  part  is  also  described  in  this  section.  Finally,  the  subroutine 
defines  configurations  for  each  border  and  stores  the  configurations  in  Q.  The  algorithm  for 
this  subroutine  is  not  presented  in  this  dissertation.  Subroutine  InitialMP  and  FinalMP  are 
for  end-portion  motion  planning  which  will  be  described  in  Chapter  V.  The  subroutine 
MidPortionMP  is  a  mid-portion  motion  planning  subroutine  which  is  presented  in  Chapter 
IV.  There  some  other  utility  functions  that  are  not  presented  in  the  dissertation  because  their 
functionality  is  pretty  straightforward.  The  meanings  of  the  functions  are  explained  as 
follows.  The  function  length  returns  the  number  of  elements  (including  regions  and 
borders)  left  in  the  global  path  class.  The  function  append  appends  a  planned  motion 
instruction  to  a  data  stmcture  MP.  The  function  car  returns  the  first  element  of  the  object 
passed  in,  while  cdr  returns  all  element  but  the  first  one. 
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IV.  MID-PORTION  MOTION  PLANNING 


A.  INTRODUCTION 

The  fact  that  the  K-regions  are  convex  polygons  makes  the  mid-portion  motion 
planning  simple  and  straightforward.  Nevertheless,  the  motion  planning  problem  has 
complex  aspects.  Safety  is  one  of  the  most  important  concerns  in  motion  planning  at  this 
stage.  The  safest  path  in  a  region  is  the  path  that  follows  the  Voronoi  boundary  [22], 
because  this  path  stays  equidistant  among  the  closest  objects.  Unfortunately,  most  Voronoi 
boundaries  cannot  be  a  part  of  a  feasible  path  for  autonomous  vehicles  with  kinematic  and 
nonholonomic  constraints.  However,  it  gives  us  the  idea  that  the  motion  will  be  considered 
safer  if  it  stays  further  away  from  objects.  Therefore,  we  propose  a  method  in  this  chapter 
which  plans  the  robot’s  motion  along  the  global  path  class  with  its  trajectory  as  close  to  the 
center  line  of  the  regions  as  possible.  How  to  plan  such  motions  for  a  rigid-body  mobile 
robot  is  the  main  subject  to  be  discussed  in  this  chapter. 

1,  Problem  Statement 

The  problem  to  be  solved  in  mid-portion  motion  planning  is  as  follows: 

Given  a  world  model,  W,  on  a  two-dimensional  plane,  91^,  and  a  global  path  class 
n=<Rii,  Bij,....,  Ri(n-i)>  Bi(n-i)’  ^in>’  ^is’  indicating  the  first  region  and  the  last 
region  which  are  considered  mid-portion  of  the  global  path  class.  The  mid-portion 
motion  planning  is  to  plan  a  safe  motion  symmetrically  for  a  rigid  body  robot  to  travel 
from  the  entrance  configuration  of  region  R^^  to  the  exit  configuration  of  region  R^^ 
along  the  global  path  class  H. 

The  inputs  to  the  mid-portion  motion  planner  are  the  world  model  W,  the  global 
path  class  U  and  the  regions  Rj^,  R^  which  indicate  the  first  region  and  the  last  region  of 
mid-portion  of  the  global  path  class.  The  outputs  are  a  sequence  of  motions  (Figure  4.1). 
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World  model  W 


Global  path 
class  n 

First  region  Rfg 
Last  region 


Mid-Portion  Motion 


Planning 


Motion  Sequence 
MPmid=  <MPi,  ...,MP„> 


Figure  4.1:  The  Mid-Portion  Motion  Planning 


2.  Definitions 

The  crossing  points  of  entrance  and  exit  borders,  P^f^,  and  and  the  border 
orientations  'Pj,,  and  define  the  entrance  configuration,  0),  and  exit 

configuration,  =  (Pouf  ^ouv  0)’  which  actually  specify  the  lines  crossing  the  entrance 
border  and  exit  border  with  border  orientations.  Although  most  of  the  crossing  points  are 
at  the  centers  of  borders,  there  are  a  few  exceptions  which  determine  other  points  on  the 
borders  as  the  crossing  points  in  the  preprocessing  of  local  motion  planning.  The  definition 
of  entrance  and  exit  configurations  will  be  intensively  used  in  this  chapter  and  in  the 
chapters  that  follow. 

In  order  to  describe  the  local  motion  planning  clearly,  we  define  some  additional 
symbols  to  represent  different  distance  measurements  in  a  K-region  based  on  the  entrance 
and  exit  borders  as  Figure  4.2.  In  this  figure,  the  line  that  is  the  bisector  of  two  Forward 
Edges  is  called  Horizontal  Center  Line,  denoted  by  HCLine  or  HCL.  The  line  that  is  the 
bisector  of  two  Cross  Edges  is  called  Vertical  Center  Line,  denoted  by  VCLine  or  VCL. 
We  can  use  configurations  to  specify  the  enter  lines.  For  instance,  the  Horizontal  Center 
Line  can  be  specified  by  the  configuration  qncune  -  ^i^where  is  the  center 

of  Entrance  Edge  and  'P,„  is  the  orientation  of  entrance  border.  The  center  line  specified  by 
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QucLine  t)®  Same  as  that  specified  by  q^J^,  depending  on  whether  the  centers  of 
borders  coincide  with  centers  of  its  corresponding  edges. 

By  djj  we  denote  the  distance  between  two  points  projected  by  the  crossing  points 
of  the  exit  border  and  entrance  border  on  the  Vertical  Center  Line.  By  <4r,„  and  we 
denote  the  closet  distance  from  the  crossing  points  of  the  entrance  and  exit  borders,  and 

to  the  Horizontal  Center  Line,  respectively.  Similarly,  rfy  denotes  the  distance  between 
two  points  projected  by  the  crossing  points  of  exit  border  and  entrance  border  on  the 
Horizontal  Center  Line.  By  and  dyout  we  denote  the  closest  distance  from  the  crossing 
point  of  entrance  and  exit  borders  and  respectively,  to  the  Vertical  Center  Line.  The 
value  of  the  djj  may  be  equal  to  the  sum  of  and  d^Q^i,  but  not  always.  For  instance,  in 
Figure  4.2,  they  are  equal.  If  the  entrance  and  exit  borders  are  on  the  same  side  of 
Horizontal  Center  Line,  they  will  be  not  equal.  The  same  relationship  is  found  among  rfyj^, 

^Vout 


Figure  4.2:  The  Definition  of  Distance  Measurements  in  a  K-region 
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B.  STEERING  FUNCTION  CHARACTERISTICS 


1.  Simulation  Results  Analysis 

Because  most  of  motion  in  a  K-region  will  be  of  the  form  of  tracking  a  reference 
line  that  is  parallel  or  perpendicular  to  the  initial  configuration,  analysis  will  focus  on  the 
simulation  of  parallel  line  tracking  and  perpendicular  line  tracking.  All  of  the  simulations 
are  done  under  the  assumption  that  the  tracking  trajectory  is  considered  to  have  converged 
if 

Ac?<cf.^;/1000,  (Eq4.1) 

where  Arf  is  the  closest  distance  from  the  current  configuration  on  the  tracking  trajectory  to 
the  reference  line  and  *^he  initial  vertical  distance. 

From  simulation  data,  (Table  4.1,  Table  4.2,  Table  4.3  and  Table  4.4),  we  found  that 
in  line  tracking  there  is  a  close  relationship  among  the  smoothness  ct,  the  vertical  distance 
difiii,  and  the  convergence  length  L.  Based  on  the  relationship  we  found,  some  rule,  that  help 
in  computing  dynamic  smoothness  in  line  tracking  can  be  developed. 


Table  4.1:  The  Relationship  among  Distances  and  Smoothness  in  Parallel  Line 
Tracking  (initial  A0  =  0)  with  Minimum  Smoothness  a 


Initial  A0 

^init 

a 

L 

0 

400.0 

70.7 

695.8 

0.18 

0 

300.0 

53.0 

521.2 

0.18 

0 

200.0 

35.4 

347.9 

0.18 

0 

100.0 

17.7 

173.28 

0.18 

0 

80.0 

14.2 

138.9 

0.18 

0 

60.0 

10.6 

103.2 

0.18 

0 

40.0 

7.1 

68.8 

0.18 

0 

20.0 

3.5 

33.1 

_ i 

0.18 
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Table  4.2:  The  Relationship  among  Distances  and  Smoothness  in  Parallel  Line 
Tracking  (initial  A6  =  0)  with  Various  Smoothness  a 


Initial  A0 

^init 

a 

L 

Lla 

0 

20.0 

3.6 

34.5 

9.58 

0 

20.0 

10.0 

109.6 

10.96 

0 

20.0 

20.0 

222.3 

11.12 

0 

20.0 

40.0 

447.1 

11.18 

0 

20.0 

60.0 

671.8 

11.20 

0 

20.0 

80.0 

896.3 

11.20 

0 

20.0 

11.21 

0 

20.0 

200.0 

2243.8 

11.22 

0 

20.0 

400.0 

4489.7 

11.22 

Table  4.3:  The  Relationship  among  Distances  and  Smoothness  in  Perpendicular  Line 
Tracking  (initial  A0  =  /  2)  with  Various  Smoothness 


Initial  A0 

^init 

a 

L 

^  /  ^init 

%/2 

100.0 

22.0 

200.0 

0.22 

n/ 2 

100.0 

40.0 

331.8 

0.40 

7C  /  2 

100.0 

41.0 

334.3 

0.41 

%  12 

100.0 

42.0 

335.5 

0.42 

71  /  2 

100.0 

43.0 

335.4 

0.43 

7C  /  2 

100.0 

44.0 

333.4 

0.44 

Til  2 

100.0 

45.0 

329.1 

0.45 

Tl  /  2 

100.0 

50.0 

246.3 

0.50 

55 


Table  4.4:  The  Relationship  among  Distances  and  Smoothness  in  Perpendicular  Line 
Tracking  (initial  A6  =  tc  /  2)  with  Various  dy  and  Corresponding  Maximum 

Smoothness  o 


Initial  A0 

^init 

a 

L 

^  !  ^init 

7C  /  2 

400.0 

168.0 

1351.7 

3.38 

nil 

300.0 

126.0 

1012.9 

3.38 

nil 

200.0 

84.0 

674.3 

3.31 

7t  /  2 

100.0 

42.0 

335.5 

3.35 

n  /  2 

80.0 

33.6 

267.9 

3.35 

nil 

60.0 

25.2 

200.0 

3.33 

nil 

40.0 

16.8 

132.4 

3.31 

n  /  2 

20.0 

8.4 

64.5 

3.23 

Several  facts  are  revealed  after  analyzing  those  simulation  data.  First  of  all,  we 
found  that  the  smoothness  applied  to  the  steering  function  in  line  tracking  can  be 
dynamically  determined  depending  on  the  following  factors: 

1.  The  vertical  distance 

2.  The  allowed  convergence  length  L. 

3.  The  initial  orientation  difference  A0  =  0^  -  0^. 

From  the  experiences  of  using  steering  function  for  motion  planning  over  years,  we 
realize  that  there  is  no  maximum  limitation  in  smoothness  of  parallel  line  tracking. 
However,  if  the  proportion  of  smoothness  to  vertical  distance  is  too  small,  the  tracking 
trajectory  will  be  unreasonable.  And  in  even  the  worst  case,  the  steering  function  cannot 
make  the  tracking  trajectory  converge  to  the  reference  line,  (see  Appendix  A)  The 
simulation  results  from  Table  4.1  show  that  for  acceptably  smooth  line  tracking,  the 
minimum  proportion  of  smoothness  versus  various  vertical  distance  is  0.18  as  Eq  4.2. 


56 


=  0.18  (Eq4.2) 

Being  aware  of  the  limitation  of  the  minimum  proportion,  we  would  like  to  compute 
the  desirable  smoothness  that  allows  the  line  tracking  trajectory  to  converge  in  limited 
length  of  reference  line.  Table  4.2  shows  a  set  of  simulation  results  using  various 
smoothness  values  (greater  than  or  equal  to  the  minimum  one).  We  found  that  for  all 
possible  smoothness  values  a  with  a  fixed  the  converge  distances  L  always  satisfies 
Eq  4.3. 

L/a<  11.22  (Eq4.3) 

Eq  4.3  suggests  that  the  maximum  proportion  of  convergence  length  to  smoothness 
CJ  is  approximately  11.2  for  all  possible  smoothness  in  parallel  line  tracking.  When  the 
smoothness  can  be  dynamically  determined,  we  want  to  obtain  the  smoothness  by  simple 
computation.  Eq  4.2  and  4.3  are  the  key  factors  in  determining  the  smoothness  of  parallel¬ 
line  tracking  in  a  specific  K-region.  We  will  discuss  this  in  the  next  subsection. 

For  perpendicular  line  tracking,  our  task  is  to  find  the  maximum  desirable 
smoothness.  The  simulation  results  is  summarized  in  Table  4.3.  The  table  shows  that  when 
the  vertical  distance  is  known,  the  most  desirable  (maximum)  smoothness  which  does 
not  lead  to  an  oscillation  convergence  is  as  Eq  4.4: 

a  =  0.42flf^  (Eq4.4) 

Although  the  Table  4.3  (based  on  =  100  cm)  is  only  a  part  of  the  simulation  results,  Eq 
4.3  is  concluded  from  all  simulations  with  various 

As  the  basic  concept  of  smoothness  shows,  in  line  tracking  using  steering  function, 
the  larger  the  smoothness  is,  the  longer  it  takes  to  converge.  When  the  length  of  the 
reference  line  is  limited,  the  smoothness  determined  by  Eq  4.3  may  not  be  able  to  make  the 
line  tracking  converge  within  the  limited  range.  Thus  the  constraint  of  Eq  4.3  should  be 
considered.  The  Table  4.4  shows  the  relationship  between  L  and  when  the  most 
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desirable  smoothness  for  each  individual  is  applied.  From  Table  4.4,  we  observed  that 
the  maximum  convergence  length  is  given  by 

L  =  3.38dj„,,  (Eq  4.5) 

Eq  4.5  can  be  viewed  as  a  lower  bound  of  convergence  length  when  the  vertical 
distance  di^i^  is  fixed  in  perpendicular  line  tracking.  Equations  4.4  and  4.5  are  the  main 
factors  of  determining  dynamic  smoothness  in  perpendicular  line  tracking.  We  will  discuss 
this  in  more  detail  in  the  next  subsection. 

2.  Dynamic  Smoothness 

For  a  specific  K-region,  motion  planning  based  on  line  tracking  needs  to  determine 
a  reasonable  smoothness  so  that  the  motion  in  that  region  is  as  smooth  as  possible  under  the 
safety  consideration.  We  discuss  the  dynamic  smoothness  determination  in  two  common 
types  of  line  tracking  in  a  K-region. 

a.  Parallel  Line  Tracking 

For  the  line  tracking  in  a  K-region,  the  length  of  the  reference  line  is  fixed. 
Thus  there  is  a  limitation  on  converge  distance.  As  previously  mentioned,  the  convergence 
length  L  in  a  region  is  actually  the  allowed  convergence  length  when  discussing 

this  subject.  We  intend  to  compute  the  smoothness  that  is  most  desirable  under  this 
limitation.  Since  the  maximum  proportion  of  convergence  length  to  the  smoothness  is 
1 1 . 22  as  the  conclusion  we  found  in  Table  4. 1 ,  the  smoothness  a  can  mainly  be  determined 
by  the  allowed  convergence  length  as  following: 

o  =  (Eq4.6) 

However,  because  of  the  characteristics  of  the  steering  function,  the  smoothness  cannot  be 
too  small  compared  with  the  initial  vertical  distance  in  the  parallel  line  tracking. 
Otherwise  its  trajectory  wiU  be  unreasonable.  Thus  we  know  the  smoothness  obtained  from 
Eq  4.2  is  the  lower  bound  of  all  possible  smoothness.  We  then  can  have  the  smoothness 
bounded  as  Eq  4.7. 
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G>0.18d.„,., 

By  replacing  a  of  Eq  4.7  with  4.6,  we  have 

(Eq4.7) 

(Eq  4.8) 

^allowed  “ 

(Eq  4.9) 

Therefore  the  smoothness  of  parallel  line  tracking  can  be  computed  as  Eq  4.6  under  the 
condition  of  >  2.02d.„ . 

The  parallel  line  tracking  will  mainly  be  applied  in  the  motion  planning  in 
the  region  with  two  parallel  borders.  In  most  cases,  L^uowed  will  be  the  length  of  the  region 
FL  in  forward  direction  of  two  parallel  borders.  This  implies  that  with  minimum  reasonable 
smoothness  the  minimum  distance  that  allows  a  parallel  line  tracking  to  converge  is  as  Eq 
4.9.  Therefore  if  FL  =  L^uowed  <  ^-02  di^ij,  it  is  not  possible  to  track  a  line  that  is  parallel  to 
the  orientation  of  the  entrance/exit  border,  from  the  center  of  entrance  border  such  that  the 
trajectory  converges  to  the  line  before  the  point  of  configuration  To  solve  the 

motion  planning  problem  under  this  situation,  an  extra  step  is  needed  in  such  a  region.  That 
is  if  FL  <2.02*  difiij,  we  will  compute  a  center  hne  between  entrance  border  and  exit  border 
that  has  its  orientation  perpendicular  to  the  borders’  orientation.  The  center  line  is  actually 
the  bisector  of  the  entrance  and  exit  borders.  Then  the  motion  in  that  region  will  become  a 
perpendicular  line  tracking  as  Figure  4.3.  The  computation  of  perpendicular  line  tracking 
will  be  described  in  next  subsection. 

b.  Perpendicular  Line  Tracking 

We  are  always  seeking  smooth  motion  in  the  motion  planning.  As  the 
analysis  in  previous  subsection,  in  perpendicular  line  tracking,  the  maximum  smoothness 
that  does  not  make  perpendicular  line  tracking  motion  oscillate  is  a  =  0.42  *  d^^  for  a 

known  vertical  distance  However,  as  in  parallel  line  tracking,  there  is  a  limitation  on 
the  length  of  the  reference  line  that  allows  perpendicular  line  tracking  motion  to  converge 
in  a  K-region.  In  order  to  make  perpendicular  line  tracking  possible  under  this  limitation. 


59 


the  configuration  where  the  line  tracking  starts  needs  to  be  determined  prior  to  the 
determination  of  smoothness.  The  Eq  4.5  gives  us  the  minimum  convergence  length  in 
general.  It  can  be  expressed  as  following; 

(Eq4.10) 

*  =(Ps 

! 

1 
I 

^init  I 


Figure  4.3:  An  Example  of  Perpendicular  Line  Tracking  on  Center  Line  in 
a  K-region. 

Eq  4.10  must  be  satisfied  in  any  case  if  perpendicular  line  tracking  is  to  be  performed. 
Because  the  reference  line  and  its  length  are  fixed  in  a  region,  the  start  configuration  must 
be  movable  to  adjust  its  distance  to  reference  line.  We  take  the  configuration  q  defined  by 
the  center  of  border  as  the  initial  position  to  measure  the  distance  The  configuration  q 
can  be  either  or  Then  the  start  configuration  can  be  decided  by  following 
algorithm: 

Qs  = 

Compute 

if  (^allowed  <  3-38  *  dinit) 
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Compute  new  start  configuration  along  the  line  specified  by  q 
such  that  —  ^allowed  ^  3.38, 

Once  the  start  configuration  is  determined,  the  smoothness  a  can  be  computed  by: 
o  =  0.42d.„, 

The  Figure  4.4  illustrates  the  perpendicular  line  tracking  in  the  case  of 
^allowed  <  3.38  *  the  beginning.  The  start  configuration  q^  =(Ps’^  s>  moved 

forward  to  the  new  start  configuration  q^^^  =  (Pnew,Q  s’  to  shorten  di^ii  so  that  the 
condition  L^uowed  -  3-38  *  is  satisfied. 


Figure  4.4:  Perpendicular  Line  Tracking  Starting  from  a  New  Configuration 
in  K-region. 

C.  PLANNING  STRATEGIES 

The  regions  on  the  global  path  class  provide  information  for  rough  navigation.  The 
safe  motion  for  accomplishing  the  mission  will  not  be  ensured  without  further  elaborative 
planning.  In  order  to  simplify  the  complex  task  in  mid-portion  motion  planning,  we  propose 
to  solve  the  problem  region  by  region  starting  from  the  first  one  of  the  mid-portion.  That  is 
to  perform  mid-portion  motion  planning  in  a  single  K-region  independent  of  the  situation 
in  the  neighboring  K-regions.  This  strategy  is  possible  as  long  as  the  motion  in  each 
individual  region  can  be  linked  by  continuous  motion  of  its  neighboring  region. 


61 


To  support  this  strategy,  one  possible  method  is  to  find  a  configuration  on  entrance 
and  exit  borders  of  a  region  and  connect  those  configurations  with  a  smooth  path.  The 
preprocessing  step  of  mid-portion  motion  planning  determined  where  to  cross  the  two 
borders  prior  the  main  planning  starts.  Those  crossing  points  together  with  their 
corresponding  border  orientations  are  used  to  define  border  configurations  individually. 
Because  the  exit  border  of  a  region  is  exactly  the  entrance  border  of  the  region  that  follows 
it  in  the  global  path  class,  planning  the  motion  between  two  configurations  of  the  borders 
links  the  motions  in  two  consecutive  regions  together.  When  the  motions  in  all  regions  of 
the  mid-portion  of  global  path  class  are  planned,  a  safe  and  smooth  path  is  linked. 

D.  TYPES  OF  COMBINED  MOTIONS 

Three  types  of  combined  motions  using  the  steering  function  are  identified  as 
useful  in  mid-portion  motion  planning.  They  are: 

•  Double  parallel-line  tracking. 

•  Double  perjjendicular-line  tracking 

•  A  parallel-line  tracking  followed  by  a  perpendicular-Une  tracking,  or  vice  versa. 

The  basic  type  of  “double  parallel-line  tracking”  motion  is  to  track  a  line  which  is 

parallel  to  the  Entrance  configuration  when  the  robot  crosses  the  entrance  border  of  the 
region.  ITien  at  certain  positions  it  starts  to  track  another  line  parallel  to  the  previous  one. 
Figure  4.5  illustrates  the  basic  double  parallel-line  tracking  motion.  Since  the  reference 
lines  will  be  computed  according  to  the  different  region  situations,  this  type  of  motion  may 
have  some  variations.  For  instance,  two  reference  lines  can  be  the  same  line.  Figure  4. 1 1 
shows  this  variation.  Some  other  variations  can  be  found  in  Figures  4. 13,  Figure  4. 14, 4. 17, 
4.18,  and  4.19. 

The  basic  type  of  “double  perpendicular-hne  tracking”  motion  is  to  track  a  line 
perpendicular  to  the  Entrance  configuration  at  the  beginning  and  then  at  certain  positions 
it  start  to  track  another  line  which  is  perpendicular  to  the  current  one.  Figure  4.6  illustrates 


62 


Figure  4.5:  The  Basic  Double  Parallel-Line  Tracking  Motion 


Figure  4.6:  The  Basic  Double  Perpendicular-Line  Tracking 


Another  type  of  motion  is  to  track  a  line  parallel  to  the  Entrance  configuration, 
followed  by  tracking  a  line  perpendicular  to  the  current  one.  or  vice  versa.  Figure  4.7 
illustrates  this  basic  one  parallel  line  tracking  followed  by  another  perpendicular  line 
tracking  motion.  As  two  parallel  line  tracking,  this  type  of  motion  have  some  variations. 
The  Figure  4.20, 4.21,  4.22,  and  4.23  are  the  examples  of  the  variations. 

Recall  that  there  are  three  major  types  on  the  positioning  of  the  entrance  and  exit 
borders  to  the  K-region  as  stated  in  Chapter  HI.  What  and  how  the  basic  motions  is  applied 
to  planning  motions  in  those  distinct  types  of  K-regions  will  be  discussed  in  detailed  in  the 
following  sections. 
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Figure  4.7:  The  Basic  Motion  of  a  Parallel  Line  Tracking  Followed 
by  a  Perpendicular  Line  Tracking 


E.  PLANNING  INTRA-REGION  MOTION  OF  TYPE  I 

Obviously,  this  type  of  region  has  two  borders  with  the  same  orientations,  which 
means  In  a  region  of  motion  Type  I,  the  basic  motion  is  a  forward  motion  of 

tracking  a  reference  line  which  has  orientation  the  same  as  entrance  /  exit  border.  This  is 
under  the  condition  that  the  reference  line  is  long  enough  for  line  tracking  motion  to 
converge.  Otherwise,  different  line  tracking  motion  will  planned.  Some  factors  should  be 
considered  in  planning  a  safe  motion  in  this  type  of  region.  They  are  the  dimension  of  the 
region,  the  t5^e  of  borders,  and  the  position  of  borders  in  their  corresponding  region  edge. 
Therefore  we  can  further  divide  this  type  of  region  into  three  different  categories  as  (i) 
Region  with  F-Borders  in  both  entrance  and  exit  borders  as  Figure  4.8.  (ii).  Region  with 
one  F-Border  and  one  P-Border  in  entrance  and  exit  as  Figure  4.9.  (iii).  Region  with  P- 
B orders  in  both  entrance  and  exit  border  as  Figure  4.10. 

I 

Entrance 

Figure  4.8:  The  Category  of  K-region  with  F-Borders  in  Both 
Entrance  and  Exit  Borders 
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1.  Region  with  Two  Full-Borders 

This  is  the  most  common  region  we  will  meet  in  K-region  decomposition.  Since  a 
K-region  is  a  rectangle  under  our  assumption,  and  since  both  entrance  and  exit  border  are 
Full-Border,  it  suggests  that  the  crossing  points  of  borders  be  the  centers  of  borders.  Thus 
the  lines  specified  by  entrance  configuration  0)  and  exit  configuration 

=  ^ out’  3re  colinear  with  Horizontal  Center  Line. 


Entrance 


Exit 


Figure  4.9:  The  Category  of  K-region  with  One  F-Border  and  One 
P-Border  in  Entrance  and  Exit  Border 


Entrance 


Figure  4.10:  The  Category  of  K-region  with  P-horders  in  Entrance 
and  Exit  Border  Both 

The  center  line  of  a  region  is  the  most  desirable  path  in  the  sense  of  safety.  Thus  the 
motion  in  this  kind  of  region  is  a  forward  motion  of  tracking  the  line  specified  by  Qo^t  from 
center  of  entrance  border.  Figure  4.11  illustrates  an  example  of  this  motion. 

2.  Region  with  One  Full-Border  and  One  Partial-Border 

The  typical  example  of  this  kind  of  region  is  illustrated  in  Figure  4.12.  In  the  figure, 
entrance  and  exit  borders  define  two  lines,  which  are  0)  and  qout  =  (BCout, 
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^ou(,  0).  The  crossing  point  of  entrance  border  can  be  the  point  other  than  the  center  of 
borders  if  forward  distance  FL  <  2.02  There  are  two  possible  cases: 

•  The  lines  specified  by  and  are  colinear. 

•  The  lines  specified  by  and  are  not  colinear. 

The  motion  planning  in  these  two  cases  will  be  discussed  in  the  following 
subsections. 


Figure  4.12:  A  Typical  Region  with  One  F-border  and  One  P- 
border  as  Entrance  and  Exit  Border. 

a.  Colinear  Entrance  and  Exit  Configurations 

In  this  case,  the  exit  configuration  must  be  defined  by  the  center  of  the 
exit  border  and  the  Entrance  configuration  can  be  defined  by  any  point  on  the  entrance 

border.  Because  the  crossing  point  of  the  entrance  border  is  predetermined  as  described  in 
Chapter  IE,  if  it  is  not  the  center  of  the  border,  it  implies  tracking  center  line  of  the  region 
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is  not  possible.  Thus  no  matter  what  the  is,  the  motion  will  be  a  forward  motion  tracking 
the  line  specified  by  the  configuration  with  smoothness  g  =  dy  I 

11.22  as  illustrated  in  Figure  4.13. 


Figure  4.13:  Forward  Motion  Tracking  the  Line 


b.  Non-Colinear  Entrance  and  Exit  Configurations 
In  the  region  which  has  non-colinear  entrance  and  exit  configurations,  the 
crossing  point  of  the  borders  must  be  the  centers  of  the  borders  and  tracking  center  line  is 
possible.  This  is  how  we  determine  the  crossing  point  in  this  kind  of  special  region.  As  the 
example  illustrated  in  Figure  4.12,  the  entrance  border  is  the  F-Border.  Thus  the  entrance 
configuration  =  (Pin’  ^in’  actually  specifying  the  center  line  of  the  region.The  most 

straightforward  motion  in  this  region  is  to  track  the  line  specified  by  qi^  at  the  beginning  of 
entering  the  region,  then  at  certain  position  leave  the  first  line  and  start  to  track  the  parallel 
line  specified  by  0).  However,  as  discussed  in  Chapter  IE,  the  trajectory 

of  directly  tracking  two  lines  will  not  be  symmetric  to  the  trajectory  traveled  by  the  motion 
of  reverse  direction.  Thus,  planning  motion  in  this  type  of  region  will  need  a  reverse  path 
Crev  as  described  in  Chapter  IE  to  support  the  symmetric  motion  planning. 

In  this  case,  the  reference  line  will  be  the  Horizontal  Center  Line  and 


=  0  so  that  we  have  The  allowed  convergence  length  will  be  L  =  dy  which  is 

the  length  of  the  Horizontal  Center  Line.  The  smoothness  can  be  computed  as  = 


dy  1 1122  (see  Eq  4.6).  Then  the  reverse  path  is  generated  by  tracking  the  Horizontal 
Center  Line  q^.  =  +  %,  0)  from  the  configuration  +  k,  0)  with 

smoothness 

Motion  in  this  region  is  executed  by  tracking  the  center  line  after  entering 
the  region  followed  by  tracking  the  reverse  path  as  Figure  4.14.  Because  the  reverse 
path  is  a  sequence  of  configurations  and  its  first  configuration  is  exactly  specifying 
a  line  the  same  as  the  configuration  q^^  specifies. Therefore,  the  entire  motion  in  this  region 
can  be  simplified  by  tracking  the  reverse  path 


Figure  4.14:  The  Motion  Planning  in  Region  with  One  F-border 
and  One  P-border  as  Entrance  and  Exit. 


For  the  case  that  the  entrance  border  is  a  P-border  and  the  exit  border  is  an 
F-border,  the  motion  in  this  region  is  simply  to  track  the  line  specified  by  from  the 
configuration  qi„.  No  reverse  path  is  needed.  Its  trajectory  will  be  exactly  the  same  as  the 
trajectory  illustrated  in  Figure  4.14  except  the  motion  orientation  is  opposite.  Therefore  the 
reverse  path  is  needed  only  when  the  center  of  exit  border  does  not  coincide  with  its 
corresponding  edge  center  in  this  kind  of  region. 

3.  Region  with  Two  Partial-borders 

There  could  be  three  situations  in  the  region  with  P-borders  in  both  entrance  and 
exit  borders,  (i).  Both  centers  of  entrance  and  exit  borders  are  aligned  with  centers  of  their 
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corresponding  edges  as  shown  in  Figure  4.15(a).  (ii).  Forward  length  of  the  region  FL  =  dy 
<  2.02  *  df]  as  shown  in  Figure  4.13(b).  (iii).  Forward  length  of  the  region  FL  =  dy>  2.02 
*  d^  as  shown  in  Figure  4.13(c).  In  the  case  of  (i),  the  Local  motion  planning  will  be  the 
same  as  the  category  in  subsection  1  of  this  section.  The  cases  (ii)  and  (iii)  will  be  described 
in  following  subsections. 


(a)  Centers  aligned  (b)  FL  =  dy  <2.02  *  %  FL  =  dy>  2.02  *  djj 

Figure  4.15:  The  Cases  of  the  Region  with  P-border  in  Both  Entrance  and  Exit 
Border 


a.  Forward  Length  FL  =  dy<  2.02  *  djj 

As  described  previously,  the  minimum  length  of  reference  line  which 
allows  parallel  line  tracking  to  converge  is  >2.02*  di^^.  In  our  case  as  Figure  4.15 

^allowed  ~  ^init  ~  Therefore,  in  this  case  since  dy  <2.02*  djj,  it  is  not  possible 

to  track  a  parallel  reference  line  and  converge  to  the  line  in  the  region.  The  motion  planning 
in  this  case  will  be  taking  the  Vertical  Center  Lined  as  reference  line  and  perform  two 
perpendicular  line  trackings.  One  from  entrance  configuration  and  another  one  from 
Exit  configuration  This  implies  dy  =  dyi^  +  dy^^t  and  dyi^  =  dy^ut-  The  later 
perpendicular  line  tracking  will  be  performed  by  generating  a  reverse  path  to  follow  so  that 
the  motion  is  symmetric.  In  order  to  ensure  perpendicular  line  tracking  convergence,  Eq 
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4. 10  is  adopted.  Because  there  will  be  two  perpendicular  line  tracking  with  same  reference 
line,  the  length  of  reference  line  that  allows  line  tracking  to  converge  will  hedfj>2*  3.38 
*  dy.  If  this  is  satisfied,  then  the  perpendicular  line  tracking  can  be  performed  from  the 
configurations  qi„  and  as  Figure  4.16(a).  Otherwise  the  start  configurations  need  to  be 
computed.  The  new  start  configurations,  and  are  on  the  line  and  q^j,t  such  that 
dy^ew  -  /  (2  *  3.38)  where  dy^^^^  is  the  distance  as  shown  in  Figure  4.16(b).  The 

smoothness  for  one  perpendicular  line  trackings  can  be  calculated  as  Eq  4.4.  In  the  case  of 
double  perpendicular  line  tracking  on  the  same  reference  line,  we  have: 

<7in  =  ctout  =  0-42  *  dy 


Figure  4.16:  The  Example  of  Double  Perpendicular  Line  Tracking 


The  motion  planning  for  the  region  in  this  case  is  as  follows: 

•  Motion  Type:  perpendicular  line  tracking: 

•  Reference  line:  Vertical  Center  Line 

•  Compute  perpendicular  line  tracking  start  configuration  q^i^^  and  on  line 
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respectively. 

•  Compute  perpendicular  line  tracking  smoothness  and 

•  Generate  reverse  path  by  tracking  from 

•  The  motion  execution  will  be:  a.  Track  the  line  from  configuration  until 
start  configuration  is  reached,  b.  Track  the  reverse  path 

b.  Forward  Length  FL  =  dy>  2.02  *  dfj 

In  the  region  with  the  forward  length  FL  =  dy  >  2.02  *<%,  a  parallel  line 
tracking  is  expected.  Ideally,  tracking  the  Horizontal  Center  Line  of  this  type  of  the  region 
will  be  the  most  desirable  motion.  However,  taking  the  center  line  as  reference  line  is  not 
always  possible  for  many  situations.  We  discuss  this  in  following  three  different  situations: 
(i).  One  of  crossing  point  of  Entrance  or  exit  borders  coincides  with  the  center  of  its 
corresponding  edge.  (ii).  Both  crossing  point  of  entrance  and  exit  borders,  are  on  the  same 
side  of  the  Horizontal  Center  Line.  (iii).  The  crossing  point  of  entrance  and  exit  borders, 
are  on  the  opposite  side  of  the  Horizontal  Center  Line  of  the  region. 

For  the  case  (i),  the  local  motion  planning  will  be  similar  to  the  motion 
planning  in  the  region  with  one  F-border  and  one  P-border  in  entrance  and  exit  border 
described  earlier  in  this  chapter.  Keep  in  mind  that  the  reverse  path  is  generated  only  if  the 
crossing  point  of  exit  border  does  not  coincide  with  its  corresponding  edge  center.  The 
followings  are  the  motion  planning  in  the  rest  of  cases. 

(1)  Crossing  Points  on  the  Same  Side  of  Center  Line:  Because  the 
crossing  points  of  two  borders  are  on  the  same  side  of  the  center  line,  to  track  the  center 
line  must  take  some  extra  efforts.  Thus  whether  to  track  the  center  line  is  an  important 
decision.  Since  the  safe  motion  is  our  major  concern  in  local  motion  planning,  and  tracking 
the  center  line  in  a  region  is  considered  the  safest  motion,  our  decision  for  this  is  to  track 
the  center  line  whenever  it  is  possible.  As  analysis  in  this  chapter  earlier,  the  converge 
length  for  a  parallel  line  tracking  is  2.02  *  di^i(.  Therefore  if  the  Eq  4.11  is  satisfied,  the 
(parallel)  center  line  tracking  motion  will  be  performed: 

11 


d^>  2.02  (d„,„ (Eq4.11) 
where  dfjin  and  are  the  distances  from  Horizontal  Center  Line  to  qi^  and 
respectively.  In  the  center  line  tracking  motion  planning,  a  reverse  path  is  always 
generated  from  the  crossing  point  of  exit  border  reversely.  Because  distances  d^in  and 
may  be  different,  the  smoothness  and  need  to  be  computed  separately  as  follows: 


r  A 

^  11.22  X  id„. 

^  Hin 


(Eq4.12) 


(Eq4.13) 


The  entire  motion  in  this  region  then  is  tracking  the  reverse  path  Cf-gy  as  Figure  4.17. 

On  the  other  hand,  if  FL=  dy  <2.02  *  (d^ij^  +  the  forward  length  of 

the  region  is  not  long  enough  for  tracking  Horizontal  Center  Line  and  converging  on  the 
line.  In  this  case,  instead  of  tracking  the  center  line,  the  reference  line  will  be  chosen 
according  the  distance  from  the  crossing  point  of  borders  to  the  Horizontal  Center  Line. 
Since  the  motion  which  is  closer  to  the  center  line  is  consider  safer,  we  take  the  line  which 
is  closer  to  center  line  as  the  reference  line.  The  smoothness  is  computed  as  Eq  4.14 

^in  =  ^oat  =  dy/n.ll  (Eq4.14) 


For  symmetry  reason,  when  the  reference  line  is  a  reverse  path  C^-gy  will 
be  generated  from  qQ^^J  with  reverse  orientation.  The  entire  motion  in  the  region  of  this 
situation  is  tracking  the  reference  line  or  the  reverse  path  C^gy  if  it  exists.  The  Figure  4.18 
illustrates  the  example. 

(2)  Crossing  Points  on  the  Opposite  Side  of  Center  Line:  In  the  region  of 
crossing  points  on  the  opposite  side  of  the  Horizontal  Center  Line,  because  the  center  line 
is  between  and  q^m,  and  the  length  of  the  center  line  is  long  enough  for  parallel  line 
tracking  to  converge,  the  motion  in  this  region  will  be  simply  tracking  the  center  line  as 
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FL=dv 


Figure  4.17:  The  Example  of  the  Motion  Tracking  the  Horizontal  Center 
Line  When  FL=  dy  ^  2.02  *  (djjifi  +  ^Hou^’ 


I  Qout 


Figure  4.18:  The  Example  of  the  Motion  Tracking  a  Reference  Line  Closer  to 
the  Horizontal  Center  Line  When  FL=  dy  <  2.02  *  +  djjg^^. 
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illustrated  in  Figure  4.19.  The  computation  of  smoothness  and  motion  planning  for  this 
situation  is  similar  to  that  of  the  first  case  of  the  region  with  crossing  points  on  the  same 
side  of  the  Horizontal  Center  Line. 


Figure  4.19;  The  Example  of  the  Motion  Planning  in  the  Region  with 
and  on  the  Opposite  Side  of  the  Horizontal  Center  Line 

F.  PLANNING  INTRA-REGION  MOTION  OF  TYPE  H 

The  characteristics  of  this  type  of  region  is  that  entrance  orientation  is  perpendicular 
to  the  orientation  of  exit  border.  Therefore  the  basic  motion  in  this  type  of  region  is  a  left 
or  right  turning  motion. 

There  are  basically  two  categories  in  this  type  of  region  which  is  separated  by  the 
relationship  between  Forward  Length  FL  and  Cross  Length  CL  of  the  region. 

1.  Forward  Length  Greater  than  or  Equal  to  Cross  Length 
In  the  region  of  FL  >  CL,  the  main  reference  line  will  be  the  Horizontal  Center  Line 
defined  by  bisector  two  Forward  Edges.  Since  the  motion  tracking  the  center  line  is  always 
considered  a  safe  motion,  we  will  plan  the  detailed  motion  which  follows  the  center  as 
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much  as  possible.  However,  as  in  other  type  of  region,  it  is  not  always  possible  to  track  the 
center  because  of  its  length  limitation.  If  tracking  the  center  line  is  taken,  the  motion  must 
be  a  parallel  line  tracking  at  the  beginning  of  entering  the  region  and  followed  by  a 
perpendicular  line  tracking  to  cross  the  exit  border.  In  order  to  plan  a  symmetric  path,  a 
reverse  path  will  be  generated  with  perpendicular  line  tracking  starting  at  exit  border.  Thus, 
both  line  trackings  take  the  center  line  as  reference  line.  This  give  us  a  hint  in  calculating 
how  long  the  center  line  is  needed  for  parallel  and  perpendicular  line  tracking  to  converge. 
As  analyzed  in  this  chapter  earlier,  the  minimum  length  on  reference  line  that  allows 
parallel  line  tracking  to  converge  is  2.02  *  di^n  and  in  this  case  we  have  =  dfji„.  The 
minimum  length  on  reference  line  that  allows  perpendicular  line  tracking  to  converge  is 
3.38  *  di^if.  In  this  case  we  have  Because  the  center  line  is  parallel  to  the  line 

specified  by  the  distance  df^  is  total  length  on  center  line  that  allows  both  line  trackings 
to  converge.  Therefore,  whether  to  track  the  center  line  in  this  kind  of  region  depends 
whether  the  following  inequality  is  satisfied.: 

dv  >  (2.02*dHi^  +  3.38*dHout)  (Eq  4.15) 


When  Eq  4.15  is  satisfied,  tracking  the  center  line  is  possible.  A  reverse  path 
will  be  generated  after  their  individual  smoothness  is  calculated  as  Eq4.16  and  4.17. 


= 


out 


dy  3.38  X 

11.22 

=  0.42xd^^„, 


(Eq4.16) 

(Eq4.17) 


Then  the  motion  in  this  region  is  to  track  the  reverse  path  as  Figure  4.20  and  Figure 
4.21. 

If  Eq  4.15  is  not  satisfied,  center  line  tracking  will  not  be  possible.  Then  a  single 
perpendicular  line  tracking  motion  will  be  planned.  Either  the  line  specified  by  entrance 
configuration  or  the  line  specified  by  exit  configuration  will  be  chosen  as  the  reference  line 
depending  on  the  length  it  allows  line  tracking  to  converge.  For  consistence,  if  the  distance 
dy  >  rf//,which  means  the  line  specified  by  will  be  longer  (measured  from  the  crossing 
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Figure  4.20:  The  Example  I  of  Tracking  Horizontal  Center  Line  in  the 
Region  of  Type  H. 


Figure  4.21:  The  Example  II  of  Tracking  Horizontal  Center  Line  in  the 
Region  of  Type  H. 

point  to  the  intersection  of  two  lines  and  Qom),  then  the  line  will  be  the  reference  hne. 
Again  for  the  symmetric  path  consideration,  a  reverse  path  may  be  generated  if  the 
reference  line  is  entrance  configuration  qi^.  With  the  reference  line  selected,  we  are  able  to 
compute  the  start  configuration  such  that  its  distance  to  reference  line  satisfies  d  = 

/  3.38.  Then  its  smoothness  for  perpendicular  line  tracking  will  be  G  =  0.42  *  d.  We 
generate  the  reverse  path  by  tracking  the  reference  line  from  with  reverse 
direction.  The  entire  motion  in  this  region  then  is  tracking  the  reverse  path  C^g^,.  Figure 
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4.22  illustrates  the  single  perpendicular  line  tracking  planning.  If  dy  <  then  the  line 
will  be  selected  as  reference  line.  The  start  configuration  and  its  smoothness  a  for 
perpendicular  line  tracking  can  be  calculated  in  similar  way.  The  motion  in  this  region  then 
is  planned  to  track  when  entering  the  region  until  reaches  q^i^.  Then  perform 
perpendicular  line  tracking  with  as  reference  line.  No  reverse  path  is  needed  in  this 
case. 


d= 

dy  1 3.38 


Figure  4.22:  The  Example  of  Single  Perpendicular  Line  Tracking  in  the 
Region  of  Type  II 


2.  Forward  Length  Smaller  than  Cross  Length 

In  the  region  of  Forward  Length  less  than  Cross  Length,  the  Vertical  Center  Line  of 
the  region  define  by  the  bisector  of  two  Cross  Edges  is  to  be  considered  as  reference  line  if 
possible.  The  local  motion  planning  rule  in  that  kind  of  region  is  similar  the  region  of  FL  > 
CL  except  the  center  line  is  defined  by  different  pair  of  edges.  Figure  4.23  illustrates  an 
example  of  the  motion  planning  in  this  region. 
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Figure  4.23:  An  Example  of  Tracking  Center  Line  Defined  by  Two  Cross 
Edges  of  the  Region  of  Type  n 

G.  PLANNING  INTRA-REGION  MOTION  OF  TYPE  HI 

In  the  region  of  the  entrance  and  exit  border  on  the  same  edge,  the  motion  can  be 
planned  with  the  combination  of  two  perpendicular  line  trackings.  The  reference  line  will 
be  the  Vertical  Center  Line  or  a  line  parallel  to  it.  In  this  type  of  region,  we  will  consider 
whether  to  track  the  Vertical  Center  Line  or  not.  Let  du,  and  be  the  distances  as 
shown  on  the  Figure  4.24.  For  a  perpendicular  line  tracking,  we  have  the  restriction  that  the 
length  on  the  reference  line  must  be  -  3-38  *  di^u  (Eq  4.10).  Thus,  if  Eq  4.18  is 

satisfied,  then  the  Vertical  Center  Line  will  be  the  reference  line  of  the  perpendicular  line 
trackings. 

du  >  (3.38  *  dyifi  +  3.38  * 

dH  >2*3.38*dvout  (Eq4.18) 

And  the  smoothness  for  the  line  tracking  is 
(Jin  =  (Jout  ~  0.42  *  dy^uf 
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For  symmetric  motion,  the  reverse  path  is  generated  by  tracking  the  center  line  from 
the  exit  configuration  with  reverse  orientations.  Then  the  entire  motion  in  this  region 
is  tracking  the  reverse  path  The  motion  of  tracking  center  line  of  this  region  is 
illustrated  in  Figure  4.24. 


Figure  4.24:  The  Motion  of  Vertical  Center  Line  Tracking  in  the  Region  of 
Motion  Type  ni. 


Figure  4.25:  The  Motion  of  a  Reference  Line  Tracking  in  the 
Region  of  Motion  Type  HI 
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For  the  case  that  dfj<2  *  3.38  instead  of  tracking  the  Vertical  Center  Line, 

the  motion  is  tracking  a  reference  line  parallel  to  the  center  line  such  that  dfj  =  2  *338 
*dvout  where  is  the  distance  between  the  reference  line  to  the  edge  containing  the 
borders.  The  rest  of  planning  in  this  region  will  be  all  the  same.  Figure  4.25  illustrates  this 
type  of  motion  planning. 

H.  MID-PORTION  MOTION  PLANNING  RULES 

The  previous  sections  analyzed  how  the  local  motion  planning  in  the  individual 
regions  is  done.  We  summarize  those  analysis  into  motion  rules  based  on  the  type  of 
regions.  Each  of  different  t5^e  of  regions  can  be  subdivided  into  two  planning  situations 
depending  on  the  region  feature  which  allows  certain  type  of  line  tracking  being  performed. 
Figure  4.26  shows  the  decision  tree  for  the  mid-portion  motion  planning  rules. 

1,  Region  with  Two  Parallel  Borders 

a.  dy  >  2.02  (djjin  + 

(1)  Motion  type:  Two  parallel  line  trackings. 

(2)  Reference  line:  Horizontal  Center  Line. 

(3)  Planning: 

(a)  Compute  smoothness: 

For  if  =  0,  -  dy!  11.22,  else  Eq  4.2 

For  if  —  0,  ^  11-22,  else  Eq  4.3 

(b)  Generate  a  reverse  path  from  using  smoothness 

(4)  Motion  execution: 

Tracking  path  with  smoothness 

(5)  Example: 

Figure  4.1 1,  Figure  4.13,  Figure  4.14,  Figure  4.17,  Figure  4.19. 
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dy  >=  2.02(dHin  +  dHoujl 


Double  parallel¬ 
line  tracking 


Type 


2.02(dHin  +  dnout)^,^ 


dy  <  2.02d{] 


Single  parallel¬ 
line  tracking 


Double 

perpendicular¬ 
line  tracking 


dy  > 

(2.02dHin  + 


dv  >=  di 


Type  II 


(2.02dHin  +  3.38dHout) 


Parallel-line  & 
Perpendicular¬ 
line  tracking 

Single 

Perpendicular¬ 
line  tracking 


dy  <dH 


(3.38dyin  -h 


dH< 

(3.38dyin  -I-  2.02dy^ 


Perpendicular¬ 
line  &  Parallel¬ 
line  tracking 

Single 

Perpendicular¬ 
line  tracking 


Type  III 


du  >=  2*3.38  dv 


dn  <  2*3.38  d^ 


Double  > 

perpendiciolar- 
line  tracking  (on 
^  VCD _ ^ 

double  ^ 

perpendicxilar- 
line  tracking  (on  a 
line  parallel  to  VCl^ 


Figure  4.26:  The  Decision  Tree  for  Mid-portion  Motion  Planning  Rules 
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b.  dy  <  2.02  (djjiji  + 

Case  1:  dy  >  2.02  djj 

(1)  Motion  type:  Single  parallel-line  trackings. 

(2)  Reference  Une: 

Qin  ^Hin  ^Houti 

Otherwise. 

(3)  Planning: 

(a)  Compute  smoothness  (5  =  dy!  11.22. 

(b)  if  d^ifi  <  djjgj^i,  generate  reverse  path  C^gy  from 

(4)  Motion  execution: 

if  dfjifi  <  dfjQjff,  tracking  reverse  path  C^gy,  otherwise  tracking 

(5)  Example: 

Figure  4.18. 

Case  2*  dy  <  2.02  djj 

(1)  Motion  type:  Double  perpendicular-line  trackings. 

(2)  Reference  line:  Vertical  Center  Line. 

(3)  Planning: 

(a)  Compute  start  configurations  and  q^g^^l  for  perpendicular-line 
tracking. 

(b)  Compute  smoothness  o  =  0.42  dy. 

(c)  Generate  reverse  path  C^gy  from  q^out- 

(4)  Motion  execution: 

(a)  Tracking  entrance  line  until  the  configuration 

(b)  Tracking  the  reverse  path  C^^y  with  smoothness  o. 

(5)  Example: 

Figure  4.16. 
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2.  Region  with  Two  Perpendicular  Borders 

a.  dy  >  djj 

(1)  Motion  type:  Parallel-line  tracking  for  entrance  motion 

Perpendicular-line  tracking  for  exit  motion 

(2)  Reference  line: 

Horizontal  Center  Line  if  dy  >  (2.02  dfji^  +  3.38  d/zour)’ 

Qifi  Otherwise. 

(3)  Planning: 

(a)  If  reference  line  is  compute  start  configurations  qsout  its 
distance  to  the  reference  line,  du^m,  for  perpendicular-line  tracking  as 
illustrated  in  Figure  4.22. 

(b)  Compute  smoothness: 

^in  =  (dy  -  3-38  df{out)  /  1 1-22,  =  0.42 

if  CjZj  <—  0,  CJjn  = 

(c)  Generate  a  reverse  path  from  using  smoothness 

tracking  reversed  reference  line. 

(4)  Motion  execution: 

Tracking  reverse  path  C,.gy  with  smoothness  a,„. 

(5)  Example: 

Figure  4.20,  Figure  4.21,  Figure  4.22. 

b.  dy  <  djj 

(1)  Motion  type:  Perpendicular-line  tracking  for  entrance  motion 

Parallel-line  tracking  for  exit  motion. 

(2)  Reference  line: 

Vertical  Center  Line  if  d^  >  (3.38  dyi^  +  2.02 

Qout  Otherwise. 
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(3)  Planning: 

(a)  If  reference  line  is  compute  start  configurations  and  dyi^  for 
perpendicular-line  tracking  similar  to  Figine  4.22. 

(b)  Compute  smoothness: 

^in  -  ^Vin'  ^out  ~  (^H  '  3-38  dyi„)  /  1 1.22. 

^out  ^out  ~  ^in- 

(c)  Generate  a  reverse  path  from  using  smoothness  tracking 
reversed  reference  line. 

(4)  Motion  execution: 

(a)  Tracking  entrance  line  until  the  configuration  q^i^. 

(b)  Tracking  the  reverse  path  with  smoothness  aj„. 

(5)  Example: 

Figure  4.23. 

3.  Region  with  Borders  on  the  Same  Edge 
fl.  djj  ^  2  *  3.38  dy 

(1)  Motion  type:  Double  perpendicular-line  trackings. 

(2)  Reference  line:  Vertical  Center  Line. 

(3)  Planning: 

(a)  Compute  smoothness  a  =  0.42  dy. 

(b)  Generate  reverse  path  from  with  smoothness  o. 

(4)  Motion  execution: 

Tracking  the  reverse  path  with  smoothness  <7. 

(5)  Example: 

Figure  4.24. 


84 


feo  djj  ^2  ^  3.38  dy 

(1)  Motion  type:  Double  perpendicular-line  trackings. 

(2)  Reference  line: 

A  line  or  parallel  to  Vertical  Center  Line  such  that  its  distance  to  the 
border  d^  =  !  {2*  3.38) 

(3)  Planning: 

(a)  Compute  smoothness  a  =  0.42  d^. 

(b)  Generate  reverse  path  from  with  smoothness  a. 

(4)  Motion  execution: 

Tracking  the  reverse  path  with  smoothness  a. 

(5)  Example: 

Figure  4.25. 

I.  ALGORITHM 

The  mid-portion  motion  planning  is  performed  region  by  region  starting  from  the 
first  region  of  global  path  class  which  is  not  included  in  initial  motion  planning,  and  ending 
in  the  region  which  is  the  last  region  on  the  path  that  is  included  in  final  motion  planning. 
The  following  algorithm  of  mid-portion  motion  planning  is  designed  for  a  single  region  on 
the  mid-portion  of  global  path  class.  Thus,  the  inputs  of  the  algorithm  are  the  entrance  and 
exit  configurations,  the  current  region  identification  and  the  world  model.  The  output  of  the 
algorithm  is  the  motion  instmctions  which  includes  a  reference  path,  named  reverse  path 
represented  by  a  sequence  of  configurations,  a  smoothness,  which  control  the  sharpness  of 
turning  while  tracking  a  reference  line,  and  a  starting  configuration  which  indicates  where 
to  start  the  path  tracking.  Based  on  the  rule  described  above,  the  algorithm  is  developed  as 
below. 
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Algorithm  MidPortionMP  (qi^,  r^„rre«;> 

Input:  entrance  configuration; 

Qouf'  ^xit  configuration; 
rcurrent- current  region; 

W:  a  world  model 
Output:  MP:  motion  instruction 


region  ^curren’ 

Qin  =  qin, 
qOut=qout; 

ComputeDis  (region,  qin,  qOut); 

if  (regionType(region)  =TypeI)  then  !*  parallel  borders  */ 

if  (dv  >—  2 .02  *  (dhin  +  dhout)  then  /*  Two  parallel  line  tracking  */ 
refLine  =  HCL  /*  Horizontal  Center  Line  *1 
ComputeSigma  (Sigmain,  SigmaOut,rulel ); 
else 


if  (dv  >=  2.02  *  dh)  then  /*  single  line  tracking  */ 

if  (dhin  <  dhout)  then  /*  select  the  line  closer  to  center  line  */ 
refLine  =  qIn 
else 

refLine  =  qOut 

ComputeSigma  (Sigmain,  SigmaOut,  rule2.1 ); 
else  /*  two  perpendicular  line  tracking  */ 

refLine  =  VCL  I*  vertical  center  line  */ 

ComputeSigma  (Sigmain,  SigmaOut, rule2.2); 

ComputeTrackConfig  (refLine,  qin,  qout); 
else  if  ( regionType( region)  =Type2 )  then  /*  perpendicular  borders  */ 
if  (dv  >=  dh)  then  /*  single  line  tracking  */ 

if(dv  >=  (2.02*dhin  +  3.38*dhout))  then  /*  HCL  tracking  (H&V)*I 
refLine  =  HCL 
else 

refLine  =  qin 

ComputeTrackConfig  (refLine,  qin,  qout); 

ComputeSigma  (Sigmain,  SigmaOut,  rule3); 
else 

if(dh  >=  (3.38*dvin  +2.02*dvout))  then  /*  VCL  tracking  (V&H)  *1 
refLine  =  VCL 

else  /*  tracking  entrance  line*! 
refLine  =  qOut 

ComputeTrackCofig  (refLine,  qin,  qout); 

ComputeSigma  (Sigmain,  SigmaOut,  ruled); 
else  if  (regionType(region)  =Type3)  then  /*  borders  on  same  edge*! 
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(36)  if  (dh  >=  3.38*  (dvin  +  dvout))  then  /*  Two  perpendicular  tracking  *1 

(37)  refLine  =  VCL 

(38)  ComputeSigma  ( Sigmain,  SigmaOut,  ruleS ); 

(39)  else  !*  tracking  a  new  line  parallel  to  VCL*t 

(40)  refLine  =  computeRefLine( ); 

(41)  ComputeSigma  ( Sigmain,  SigmaOut,  ruled ); 

(42 )  MP.rvsPath  =  genRvsPath  (refLine,  qOut,  SigmaOut) 

(43)  MP.sigma  =  Sigmain; 

(44)  MP.startConfig  =  qin; 

(45)  return  ( MP ) ; 


The  subroutine  ComputeDis  computes  all  distance  measurements  in  a  given  region 
as  defined  in  Section  A.  The  measurements  include  dv,  dh,  dvin,  dvout,  dhin,  and  dhout, 
Subroutine  regionType  takes  region  name  as  input  and  returns  the  type  of  region  which  is 
Typel,  Type2  and  T3^e3  as  described  in  Chapter  HI  Section  C.  Subroutine  ComputeSigma 
computes  smoothness  according  to  different  region  situations  which  are  described  in 
Section  H.  The  subroutine  ComputeTrackCofig  computes  the  start  configurations  of  path 
tracking  if  necessary.  The  situations  that  need  to  compute  start  configuration  of  path 
tracking  are  described  in  Section  H.  The  subroutine  genRvsPath  generates  a  reverse  path 
represented  by  a  sequence  of  configurations  according  to  given  reference  line,  starting 
configvnation,  and  smoothness.  The  reverse  path  generation  uses  forerunner  simulation  as 
tool.  The  details  of  reverse  path  generation  are  described  in  Section  E  and  F  of  Chapter  ni. 
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V.  END-PORTION  MOTION  PLANNING 


The  end-portion  motion  planning  refers  to  initial  motion  planning  and  final  motion 
planning.  Let  and  qg  be  the  start  and  goal  configurations  respectively.  Assume  there  is  a 
collision-free  motion  planning  from  q^  to  qg,  with  path  nj.  In  the  sense  of  symmetric 
motion  planning  we  stress  in  this  dissertation,  if  the  orientations  of  all  configurations  are 
reversed,  the  path  112  of  Ihe  motion  planned  from  the  final  configuration  to  initial 
configmation  will  be  identical  to  Hi.  Therefore  the  planning  for  the  initial  motion  would 
be  similar  to  that  of  the  final  planning  except  the  they  are  reverse  in  orientations.  The 
chapter  addresses  how  the  end-portion  motion  will  be  planned. 

A.  PROBLEM  STATEMENT 

Since  the  two  plannings  in  end-portion  motion  planning  are  actually  the  mirror  of 
each  other,  the  problem  to  be  solved  in  final  motion  planning  is  the  same  as  that  of  initial 
motion  planning.  We  describe  the  initial  motion  planning  problem  as  the  following,  so  that 
the  final  motion  planning  problem  becomes  trivial. 

Given  a  world  model  with  decomposed  K-regions  on  a  two  dimensional  plane,  91^,  and 
a  global  path  class  n  =  </?,;,  Bn,....,  Rin-j,  5^-7,  Bm>’  start  configuration  q^  and 
goal  configuration  qg.  The  crossing  points  of  borders  are  determined  initially.  The  ini¬ 
tial  motion  planning  problem  is  to  determine  whether  there  exists  a  collision-free 
motion  for  a  rigid  body  robot  to  move  from  q^  to  the  regions  next  to  the  initial  one  cross¬ 
ing  the  border  perpendicularly.  If  so,  plan  a  safe  motion  symmetrically. 

B.  PLANNING  METHODS 

The  inputs  to  the  end-portion  motion  planning  are  the  initial  mission  parameters,  i.e. 
start  configuration,  q^,  goal  configuration  qg,  and  the  world  model,  W,  and  the  result  of 
global  path  planning,  i.e.  the  global  path  class  If.  The  output  of  this  planning  is  a  sequence 
of  motions  instructions,  =  <MPj,...,  MP^>,  to  be  taken,  where  n  >  7  and  each  MP,- 
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includes  type  of  motion  (either  forward  or  backing-up),  a  reference  path,  the  configurations 
to  initialize  and  end  the  motion.  Figure  5.1  illustrates  this  idea. 


Mission  parameters 
qg,  Qg  and  W 

Global  path  11 


End-Portion  Motion 


Planning 


Motion  Sequence 

MPend  =  <MPi,...MPn> 


Figure  5.1:  The  End-Portion  Motion  Planning 

According  to  these  I/O  requirements,  the  end-portion  motion  planning  will  be 
designed  to  use  methods  described  in  this  section  to  accomplish  the  planning  job.  The 
Forerunner  Simulation  has  the  key  role  in  the  entire  end-portion  motion  planning.  How  to 
apply  the  Forerunner  Simulation  to  end-portion  motion  planning  is  described  in  Subsection 

1.  To  plan  a  collision  free  motion  is  the  main  purpose  of  end-portion  motion  planning. 
Therefore,  collision  detection  is  one  of  important  topics  that  will  be  discussed  in  Subsection 

2.  In  the  simulation,  if  no  forward  motion  is  possible,  backing  up  motions  will  be  planned 
as  a  supplemental  motion.  We  discuss  this  in  Subsection  3. 

1.  Forward  Forerunner  Simulation  Application 

The  generic  Forerunner  Simulation  template  is  presented  in  Chapter  VI.  In  this 
chapter  we  discuss  how  it  is  applied  to  a  forward  simulation  in  the  end-portion  motion 
planning.  The  path  tracking  technique  is  one  of  good  methods  in  solving  local  motion 
planning  problem.  Because  the  Forerunner  Simulation  is  basically  using  line  tracking 
method  to  simulate  a  real  robot’s  motion,  we  found  it  is  especially  useful  in  end-portion 
motion  planning. 
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Given  an  initial  configuration  6j„;,  a  reference  path  specified 

by  the  configuration  path  =  ((Xp,  yp),  6^,  kp)  and  a  smoothness  a,  what  we  expect  the 
Forerunner  Simulation  to  do  is  to  perform  a  forward  line  tracking  with  following  output: 

•  If  the  forerunner  ’  s  trajectory  converges  to  the  reference  line  before  the  point  on 
the  line  where  it  is  supposed  to  converge,  output  the  line  tracking  trajectory 
from  qlf^l  to  the  configuration  where  it  converges.  Otherwise  indicates  not 
converging  case. 

•  If  collision  happens,  output  a  collision  signal. 

With  these  expectation,  a  forerunner  simulator  can  be  constructed  by  modifying  the 
simulation  template  in  Figure  3.6  of  Chapter  HI.  Figure  5.2  and  5.3  show  the  algorithms. 

As  aforementioned,  the  Forerunner  Simulation  can  be  applied  to  many  different 
applications.  In  the  ForwardSimulation  application,  the  related  codes  are  inserted  to  the 
template.  In  Figure  5.2,  line  (1),  (2),  and  (3)  are  the  PREwork  and  line  (8)  to  (14)  are  the 
application  codes.  The  collision  checking  function  in  line  (8)  will  be  discussed  later  in 
Subsection  3.  The  function  overImage()  in  line  (10)  is  to  check  the  image  [19]  of  current 
configuration  on  the  reference  line  with  the  point  where  the  trajectory  is  supposed  to 
converge  before  it.  If  the  image  runs  over  the  point,  the  function  returns  TRUE,  otherwise 
FALSE  is  returned.  The  algorithm  for  the  function  overImage()  is  omitted.  The 
convergence  checking  algorithm  in  line  (11)  is  presented  in  Figure  5.3.  The  algorithm  of 
ForwardSimulation  returns  a  path  containing  a  sequence  of  configurations  of  the 
tracking  trajectory.  Two  other  boolean  variables,  FWcollide  and  FWPerfectConverge,  are 
also  updated  and  returned.  FWcollide  indicates  whether  the  simulation  collides  with 
objects  and  FWPerfectConverge  indicates  whether  the  trajectory  converges  before  the 
point  specified  in  the  reference  line. 
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Algorithm  ForwardSimulation  (q,  a,W} 

Input:  q:  an  initial  configuration;  q^^:  a  reference  path;  g:  a  smoothness; 

W;  a  world  model; 

Output:  update  forward  path  update  FWcollide  and  FWPerfectConverge; 

(1)  Initialize  path  ny^  =  NULL; 

(2)  Initialize  FWPerfectConverge  =  FALSE; 

(3)  Initialize  overRun  =FALSE; 

(4)  Qnew  ~  1’ 

(5 )  Compute  constants  of  steering  function; 

(6)  while  ( not  FWcollide  and  not  overRun  and  not  converge ) 

qnew  =  AdvanceForerunner  ( q„g^  qp^); 

(8)  FWcollide  =CollisionChecking  ( q^^^  W); 

(9)  if  (not  FWcollide)  then 

( 10)  overRun  =  overlmage  ( q^^^  qp^); 

( 11)  FW converge  =  ConvergeC becking  ( q^^^  qp„); 

(12)  if  (not  overR un  and  converge )  then 

(13)  FWPetfectConverge  =TRUE; 

( 14)  Append  q^^^  to  Up^; 

(15)  end  while; 

Figure  5.2:  The  Algorithm  for  Forward  Forerunner  Simulation  Applied  in 
End-Portion  Motion  Planning. 


Algorithm  ConvergeChecking  (q,  q^^fi 
Input:  q:  a  configuration;  q^gf  a  reference  path; 
Output:  TRUE  /  FALSE 

(1)  Ad  =  \the  closest  distance  from  q  to  path  q^g^ 

(2)  A0  =  ie-0,,^1 

(3)  Ak=\k-krgf\ 

(4)  if  (Ad  <  e  and  A  0  <  e  and  Ak  <z)  then 

(5)  return  (TRUE); 

(6)  else 

(7)  return  (FALSE); 


Figure  5.3:  The  Algorithm  for  Convergence  Checking  in  Path  Tracking. 
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2,  Collision  Detection 

While  the  forerunner  is  running  in  end-portion  motion  planning,  collision  detection 
is  being  performed  in  each  advancing  step.  This  is  rather  an  important  step  in  motion 
planning  because  the  safety  of  the  planned  motion  relies  on  it  completely.  In  this 
dissertation,  we  assume  that  the  robot  is  an  autonomous  vehicle  with  rigid  body.  A  convex 
hull  [23]  can  be  constructed  to  enclose  the  shape  of  the  robot.  In  motion  planning,  we 
compute  the  robot’s  current  configuration  based  on  its  center.  Thus  each  vertex  of  the 
convex  hull  can  be  calculated  by  adding  the  offsets  to  the  coordinates  of  the  robot’s  center. 
As  defined  in  Chapter  V,  the  free  space  is  the  compliment  of  the  union  of  obstacles  in  the 
world.  Thus  the  robot’s  motion  is  considered  collision  free  if  and  only  if  all  vertices  of  the 
convex  hull  fall  into  the  free  space. 

Therefore  in  local  motion  planning,  instead  of  checking  possible  collision  with 
arbitrary  obstacles,  the  collision  detection  task  can  be  carried  out  by  checking  the  convex 
hull  with  decomposed-regions.  If  all  vertices  of  convex  hull  are  inside  of  the  regions  in  each 
sampling  step,  the  collision  will  never  happen.  Now  it  comes  to  the  question:  Is  it  necessary 
to  require  the  robot’s  motion  having  its  trajectory  always  fallen  into  the  regions  on  the 
global  path  class?  Ideally,  the  robot’s  trajectory  is  passing  through  region  by  region  along 
the  global  path  class.  For  instance,  if  the  global  path  class  contains  the  regions  Rp  R2,  -, 
Rji,  the  robot  should  move  from  the  initial  region  Ri  then  i?2— >  and  finally  stops  at  the 
final  configuration  in  the  region  Rj^.  The  robot  is  not  expected  to  run  into  a  region  which  is 
not  in  the  sequence  of  the  global  path  class.  However,  there  will  be  some  exceptions  at  end- 
portion.  Figure  5.4  illustrates  an  example  of  the  robot’s  motion  in  the  initial  region.  In  the 
figure,  the  region  is  the  initial  region  and  Rj  is  its  next  region  in  the  global  path  class,  but 
the  region  Rjf_  is  not  any  region  in  the  path.  The  broken  curve  shown  on  the  figure  is  a 
possible  traveling  trajectory  from  the  initial  configuration  to  the  next  region.  Although 

93 


part  of  this  trajectory  falls  into  the  region  which  is  not  the  region  in  the  path,  the  motion 
traveling  along  this  trajectory  is  obviously  a  safe  one  and  probably  a  desirable  one.  This 
example  suggests  that  to  plan  a  safe  motion,  it  is  not  necessary  to  keep  the  trajectory  always 
in  the  regions  listed  on  the  global  path  class. 

Since  the  motion  is  continuous,  if  the  sampling  step  is  small  enough,  the  robot’s 
position  change  will  be  relatively  small  in  either  real  time  or  simulation.  Thus,  we  can 
assume  that  the  next  position  is  either  in  the  current  region  or  the  region  adjacent  to  the 
current  one.  The  current  region,  to  which  the  robot’s  current  position  belongs,  and  the 
regions  adjacent  to  the  current  one,  are  considered  related  regions.  We  conclude  that 
collision  detection  can  be  done  by  checking  robot’s  position  with  the  related  regions  in  each 
sampling  step.  If  robot’s  position  is  in  one  of  those  related  regions,  it  is  appropriate  to 
declare  that  there  is  no  collision  in  current  step.  Figure  5.5  shows  the  algorithm  for  the 
collision  detection. 


Figure  5.4:  An  Example  of  the  Possible  Motion  Planning  in  the 
Initial  Region. 
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Algorithm  collisionchecking  (q,  W) 

Input:  q:  a  configuration;  W:  a  world  model; 
Output:  TRUE  /  FALSE 

(1)  Find  the  region  R  of  current  configuration  q; 

(2)  if  (no  region  found)  then 

(3)  return  (TR  UE); 

(4 )  for  all  regions  Rj  adjacent  to  R  do 

(5)  if  q  in  region  Rj  then 

(6)  return  ( FALSE ) ; 

(7)  end  for; 

(S)  return  (TRUE); 


Figure  5.5:  The  Algorithm  for  Collision  Detection. 


3.  Backing-up  Motion  Simulation 

A  forward  motion  is  always  preferable  in  motion  planning.  However,  in  some 
situation,  forward  motion  may  not  be  able  to  accomplish  the  task  at  the  end-portion  of 
entire  motion  planning.  For  instance,  if  all  possible  forward  motion  can  not  avoid  collision 
with  the  obstacles,  then  other  solutions  will  be  needed.  For  a  robot  with  nonholonomic 
constraints,  the  backing-up  motion  is  the  only  solution.  Figure  5.6  illustrates  the  situation 
in  which  the  backing-up  motion  is  required. 

The  Forerunner  Simulation  is  applied  in  forward  motion  simulation  as  described  in 
Subsection  1  of  this  section.  The  backing-up  motion  simulation  can  also  be  constructed  by 
modifying  Forerunner  Simulation  template  in  Figure  3.14.  The  piupose  of  backing-up 
motion  simulation  is  to  find  the  positions  along  the  backing-up  trajectory  where  a  collision- 
free  forward  motion  can  start.  For  this  purpose,  in  constructing  backing-up  motion 
simulation  the  followings  are  considered: 

•  Backing-up  motion  itself  must  be  a  collision  free  motion. 
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•  In  each  sampling  step,  the  forward  forerunners  should  be  issued  to  check 
whether  it  is  possible  to  have  a  collision-free  forward  motion  from  the  current 
configuration,  while  simulation  is  running  backward.  The  Figure  5.7  illustrates 
the  backing-up  simulation  with  forward  forerunner  simulation. 


Figure  5.6:  Unavoidable  Collision  with  Forward  Motion  in  the 
Initial  Region. 

While  the  simulation  is  running,  both  backward  trajectory  and  forward  trajectory 
are  saved  for  planning  use.  Although  the  forward  forerunner  may  complete  a  full 
simulation  in  each  backing-up  step,  only  the  last  forward  trajectory  will  be  kept. 

The  backing-up  motion  simulation  terminates  in  one  of  following  conditions: 

•  When  backing-up  motion  collides  with  obstacles. 

•  When  the  virtual  robot’s  orientation  is  identical  to  the  orientation  of  its  main 
(forward  forerunner’s)  reference  line. 

•  When  the  forward  forerunner  converges  to  it  reference  line  before  the  point  the 
reference  line  specified. 
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Figure  5.7:  Backing-up  Motion  Simulation  with  Forward 
Forerunner  Simulation. 


If  the  backing-up  simulation  terminates  with  collision  detected,  it  suggests  that 
more  forward  or  backward  motions  will  be  needed  to  complete  the  planning.  We  will 
discuss  this  in  more  detailed  in  Section  B. 

The  second  termination  condition  stops  the  backing-up  simulation  when  the 
orientation  of  the  virtual  backing-up  robot  reaches  the  same  orientation  of  forward 
reference  line.  Then  the  forward  motion  can  be  planned  to  track  the  line  specified  by  the 
last  configuration.  Even  though  this  motion  may  not  cross  the  exit  border  at  the  originally 
planned  crossing  point,  it  will  be  an  acceptable  safe  motion.  This  is  because  it  crosses  the 
border  with  an  orthogonal  orientation,  so  that  the  motion  in  the  next  region  can  be  planned 
accordingly  without  any  unexpected  interference.  Figure  5.8  illustrates  this  case. 

The  third  termination  condition  will  be  met  when  the  forward  forerunner  simulation 
makes  its  tracking  trajectory  converge  to  the  main  reference  line  before  the  point  its 
configuration  defines.  This  kind  of  convergence  is  called  a  perfect  convergence  because  a 
forward  motion  then  can  be  planned  to  track  the  reference  line  from  the  configuration  the 
backing-up  motion  reaches.  This  forward  motion  will  be  able  to  cross  the  exit  border  at  the 
planned  crossing  point.  The  trajectory  %f2  in  Figure  5.7  is  an  example  of  this  case. 
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Figure  5.8:  Backing-up  Motion  Simulation  Terminates  at  the 
Configuration  Where  Its  Orientation  Is  Identical  to  the  Exit 
Orientation 


Figure  5.9  and  Figure  5.10  show  the  backing-up  motion  simulation  algorithms.  The 
algorithm  for  subroutine  BackUpForerunner  in  Figure  5. 10  is  similar  to  the  algorithm  for 
AdvanceForerunner  in  Figure  3.15.  The  differences  are  (i).  in  line  (1)  of  Figure  5.10,  the 
curvature  change  will  be  computed  by  Eq  3.6  and  Eq  3.7  based  on  planning  a  backward 
motion.  Thus  the  virtual  robot’s  orientation  must  be  reversed.  As  a  result,  the  orientation  9 
in  Eq  3.7  will  be  replaced  by  0  +  it  in  computing  the  curvature  change,  dk  /  ds.  (2).  In  line 
(2),  computing  new  curvature  and  orientation  change  by  Eq  3.8  and  Eq  3.9,  the  increment 
step  Ay  of  Eq  3.8  and  Eq  3.9  has  to  be  a  negative  value  to  make  the  position  change 
backward.  The  rest  of  computations  are  the  same  as  described  in  Chapter  III  Section  E.2. 
The  subroutine  ForwardSimulation  in  Figure  5.9  is  presented  in  Figure  5.2. 

C.  INITIAL  MOTION  PLANNING 

In  Chapter  ni  and  IV,  we  describe  mid-portion  motion  planning  in  a  single  region. 
This  is  obviously  a  workable  solution,  because  the  entire  motion  is  linked  by  the  motion 
passing  through  crossing  point  of  borders  shared  by  neighboring  regions  in  the  global  path 
class.  When  we  discuss  the  end-portion  motion  planning,  we  notice  that  it  may  involve  not 


only  the  initial  or  final  region  but  also  the  regions  next  to  initial  or  final  one.  This  is  because 
the  initial  or  final  configuration  is  arbitrary.  Therefore,  the  robot  moving  from  the  initial 
region  to  the  next  one  does  not  necessarily  cross  the  first  border  at  its  center  and  with 
orthogonal  orientation.  This  would  be  a  conflict  with  essential  idea  of  mid-portion  motion 
planning.  When  this  does  happen,  the  motion  planning  in  the  region  next  to  the  initial 
region  or  final  region  will  be  considered  a  part  of  end-portion  motion  planning. 


Algorithm  BackUpSimulation  (q,  qp^  a,  W) 

Input:  q:  an  initial  configuration;  qj,^:  a  reference  path  for  backing-up  simulation; 

q^:  a  reference  path  for  forward;  a;  a  smoothness;  M:  a  world  model; 
OutpuUThe  set  of  data  including  the  path  of  backward  motion,  the  path  of  forward 
motion,  and  collision  indicator  for  backward  motion. 

(1)  Initialize  FWPerfectConverge  =  FALSE; 

(2 )  Initialize  BWcollide=  FALSE; 

(3)  Initialize  BW finished  =  FALSE; 

(^)  Q.new  ~ 

(5)  Compute  constants  of  steering  function  with  smoothness 

(6)  while  ( not  BW  collide  and  not  BW  finished  and  not  FWPerfectConverge ) 

(7)  qnew  =  BackUpForerunner  (qmw’Qbw): 

(8)  BW collide  =  Collisionchecking  ( W ) ; 

(9)  if  ( not  BW collide )  then 

(10)  if(Q^^^:^Qf^)then 

( 11)  ForwardSimulation  (q^e^  Qfw’ 

(12 )  else  BW  finished  =  TRUE; 

(13)  Append  q^^^  to  path 

(14)  end  while; 

Figure  5.9:  The  Algorithm  for  Backing-up  Motion  Simulation  in  End- 
Portion  Motion  Planning. 

Based  on  the  line  tracking  of  steering  function,  the  Forerunner  simulation  described 
in  Section  A  are  applied  to  the  initial  motion  planning.  Let  q^  =  (p^,  6^,  kfi  be  the  start 
configuration,  and  q^^  =  ( CP()^^^,  k^^^f  be  the  configuration  defined  by  the  crossing 

point,  PCgj^p  and  orientation,  of  the  exit  border  in  initial  region.  Let  dy  denote  the 


closest  distance  between  start  configuration  and  the  reference  line.  (For  example  in 
Figure  5.1 1  reference  line  is  the  line  defined  by 


Algorithm  BackUpForerunner  (q,  q^^j-) 

Input:  q:  the  current  configuration;  q^^j-:  the  reference  path; 
Output:  next  configuration 

(1)  Compute  curvature  change  in  backward  direction; 

(2 )  Compute  new  curvature  and  orientation  change; 

(3)  Compute  new  configuration  of  next  step; 

(4)  return  ( new  configuration ) 


Figure  5.10:  The  Algorithm  for  Backing-up  Forerunner  in  One  Step. 


Figure  5.11:  An  Example  of  the  Initial  Region. 

The  initial  motion  planning  will  be  performed  by  following  basic  steps; 

•  Determine  a  reference  line  q^^j-. 

•  Determine  smoothness  a. 

•  Run  forward  Forerunner  Simulation. 

•  If  no  collision-free  forward  motion  is  possible,  run  backing-up  motion 
simulation. 
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1.  Determination  of  Reference  Line 

The  reference  line  is  used  to  drive  the  virtual  robot  to  a  desired  direction  in 
Forerunner  Simulation.  In  the  initial  motion  planning,  the  crossing  position  of  the  exit 
border,  CP^nt,  is  predetermined  (in  most  cases,  it  is  the  center  of  the  border).  With  the 
orientation  of  exit  border,  it  defines  the  exit  configuration  It 

is  desirable  to  plan  a  motion  that  leaves  the  region  at  exit  configuration  (at  the  position 
and  with  orientation  because  the  exit  configuration  in  this  region  is  exactly  the 
entrance  configuration  of  next  region  on  the  path.  Thus  the  line  specified  by  the  exit 
configuration  of  the  initial  region  will  be  the  first  choice  in  determining  reference  line  of 
forward  Forerunner  Simulation.  In  some  cases,  we  may  need  to  adopt  a  reference  line  other 
than  the  exit  configuration  of  initial  region.  In  this  dissertation,  four  different  lines  will  be 
considered  useful  in  end-portion  motion  planning.  We  define  these  lines  as  follows: 

*  -  ^out-inii-  "I^®  specified  by  the  exit  configuration  of  initial  region. 

•  <3'2  ’  ^  li*^®  which  passes  through  the  position  of  initial  configuration  and  is 
perpendicular  to  the  qout-init- 

*  ^3  -  ^out-next-  Th®  H^®  Specified  by  the  exit  configuration  of  the  region  next  to 
the  initial  region. 

•  Q4  =  Qref-next-  The  line  computed  as  the  reference  line  in  the  region  next  to  the 
initial  one  by  the  method  described  in  mid-portion  motion  planning. 

The  line  qj  and  q2  are  illustrated  in  Figure  5.12(a).  The  figure  shows  the  examples 
of  two  different  start  configurations,  q^j  and  q^2->  which  need  different  reference  line  in 
motion  planning.  The  Figure  5.12(b)  illustrates  the  situation  that  reference  line  q^  and 
are  adopted.  In  the  figure  Rinn  is  the  initial  region  where  the  start  configuration  locates, 
and  the  region  Rn^xt  is  the  region  next  to  the  initial  region  on  the  global  path  class. 

In  this  dissertation  a  line  is  specified  by  a  configurations  defined  as  ^  =  (p,  6,  k), 
where  p  can  be  any  point  on  the  line  it  defined.  However,  for  planning  convenience,  when 
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a  line  is  adopted  as  the  reference  lines  in  line  tracking,  we  would  like  to  use  a  more  specific 
configmation  to  specify  this  line.  That  is  the  point  p  that  will  be  the  farthest  point  on  the 
line  where  we  expect  a  line  tracking  to  converge.  The  reference  line  candidates  qj  and 
are  on  the  borders  of  the  regions.  The  points  on  the  specifying  configuration  are  already  the 
farthest  points  on  the  lines,  because  beyond  those  points  the  lines  will  be  out  of  their 
regions.  For  line  <72  and  ^4,  the  expected  converge  points  can  be  calculated.  The  Figure  5.13 
illustrates  how  the  expected  converge  point  for  line  ^2  is  calculated.  The  line  ^2  is  designed 
as  a  line  perpendicular  to  the  line  specified  by  q^j^.  Its  orientation  can  be  computed  as  02  = 


Figure  5.12:  The  Reference  Lines  in  the  Initial  Motion  Planning. 
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For  a  smooth  motion  planning  in  the  example  of  Figure  5.13,  eventually  there 
would  be  a  perpendicular  line  tracking  from  a  point  P2  on  ^2  to  line  as  indicated  in  the 
figure.  For  perpendicular  line  tracking  using  minimum  desirable  smoothness,  the  distance 
to  converge  is  twice  of  the  distance  from  the  starting  position  to  the  reference  line  as  shown 
on  Table  4.3  and  Table  A.2.  Since  the  line  is  also  designed  to  pass  through  the  initial 

position,  the  distance  to  converge,  d,  can  be  calculated  easily.  As  a  result,  the  point  P2  is 
then  computed  easily.  Therefore  the  line  q2  is  defined  as  (P2,  ^Vout  +  tt/2,  0)  in  the  example 
of  Figure  5.13. 

The  reference  lines  described  above  are  for  the  forward  Forerunner  Simulation. 
Once  the  reference  line  is  determined,  with  proper  smoothness  the  simulation  starts  a 
forward  Forerunner  Simulation.  If  the  forward  simulation  can  not  complete  the  planning, 
then  the  backing-up  motion  simulation  will  be  issued.  As  forward  one,  the  backing-up 
motion  simulation  needs  a  reference  line  to  start  its  work.  Since  the  backing-up  simulation 
is  a  supplemental  tool  whose  main  task  is  to  make  forward  simulation  possible  to  fulfill  the 
end-portion  motion  planning,  the  forward  reference  line  can  be  considered  as  its  goal.  So 
that  while  the  backing-up  simulation  is  running,  the  virtual  robot’s  orientation  is  changing 
and  gradually  heading  toward  the  direction  of  the  forward  reference  line.  This  suggests  that 
the  reference  line  for  backing-up  motion  simulation  must  have  its  orientation  opposite  to 
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the  orientation  of  forward  reference  line.  We  conclude  this  subsection  with  defining  the 
reference  line  for  backing-up  motion  simulation  as: 

^backup  ~  (Pi’  ^forward  5.1) 

where  is  the  position  of  start  configuration,  and  ^forward  t^e  orientation  of 
predetermined  forward  reference  line. 

2.  Dynamic  Smoothness  in  Initial  Motion  Planning 

In  order  to  simply  the  initial  motion  planning  among  various  situations  with  start 
configuration,  the  minimum  desirable  smoothness  will  be  computed  dynamically 
depending  on  the  relationship  between  the  position  of  start  configuration  and  the  reference 
line.  As  shown  on  Table  A.l  and  Table  A.2  in  Appendix  A,  the  Max-Min  smoothness  for 
various  orientation  is 

a  =  0.22  *  dy  (Eq  5.2) 

where  the  dy  is  the  closest  distance  from  the  start  configuration  to  the  reference  line.  The 
smoothness  computed  by  Eq  5.2  may  satisfy  most  of  line  tracking  cases.  However,  the 
distance  dy  can  be  very  small,  even  close  or  equal  to  zero,  but  the  smoothness  used  in 
steering  function  can  not  be  too  small.  Therefore  a  lower  bound  of  minimum  smoothness 
is  required.  According  to  our  experience  in  using  steering  function,  the  smallest 
smoothness  that  makes  line  tracking  work  is  twice  as  much  as  the  distance  the  robot 
advance  in  a  motion  control  cycle  (a  step  in  simulation).  For  instance  if  each  step  is  0. 1  cm, 
then  the  minimum  smoothness  is  a  =  0.2.  As  we  know,  the  smaller  the  smoothness  is,  the 
sharper  its  turning  will  be.To  avoid  unreasonable  sharpness,  we  set  the  overall  minimum 
smoothness  to  be 

^min  ~  ^ 5.3) 
Therefore,  the  smoothness  is  determined  as  Eq  5.4 

a  =  0.22  *  dy)  (Eq  5.4) 
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3.  Motion  Planning  Simulation 

Since  four  lines  can  be  taken  as  a  reference  line  for  forward  forerunner  simulation 
in  initial  motion  planning,  we  will  describe  the  motion  planning  simulation  with  those 
different  reference  lines  individually  in  this  section.  In  addition,  the  backing-up  motion 
planning  is  also  discussed  in  detailed  in  this  section.  We  name  the  motion  planning 
simulations  as  follows: 

•  The  First  Planning  Simulation. 

•  The  Second  Planning  Simulation. 

•  The  Third  Planning  Simulation. 

•  The  Fourth  Planning  Simulation. 

•  The  Backing-up  Motion  Planning  Simulation. 

The  initial  motion  planning  begins  with  First  planning  simulation,  then  the  other 
planning  simulations  may  follow  if  necessary. 

a.  The  First  Planning  S imulaiion 

This  forward  forerunner  simulation  takes  the  line  specified  by  the  exit 
configuration,  qj,  of  initial  region  as  its  reference  line.  In  the  forward  forerunner 
simulation,  the  alternative  tracking  methods  in  Two-way  Line  Tracking  (Appendix  A)  will 
be  tried  after  the  normal  tracking  fails  to  find  a  motion  with  its  trajectory  perfectly 
converging  to  the  reference  line.  If  a  trajectory  with  perfect  convergence  and  no  collision 
is  found,  the  First  planning  simulation  is  done.  Then  the  motion  will  be  planned  to  track  the 
line  Qi  directly.  Figure  5.14  illustrates  this  planning. 

If  there  is  a  collision  in  the  First  planning  simulation,  we  may  need  to  try 
other  simulations  which  is  the  Second  Planning  Simulation.  We  will  discuss  this  simulation 
later  in  this  section. 

If  this  simulation  has  no  collision  detected,  but  its  trajectory  does  not 
converge  in  the  region.  This  suggests  that  the  start  configuration  is  close  to  the  exit  border. 
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It  may  be  better  to  track  a  reference  line  in  the  region  next  to  the  initial  one.  Therefore  the 
Third  Planning  Simulation  follows  this  case. 


Figure  5.14:  An  Example  of  Forward  Forerunner  Simulation  with  No 
Collision  and  Converging  in  the  Initial  Region. 


b.  The  Second  Planning  Simulation 

Figure  5.15  shows  the  situations  when  a  collision  will  be  detected  in  forward 
forerunner  simulation.  In  the  figure,  q^i  and  q^2  represent  start  configuration  in  two 
different  case.  In  the  case  with  start  configuration  q^i,  the  image  of  start  configuration  on 
the  exit  edge  falls  on  the  border.  When  there  is  a  collision  in  forward  simulation,  there  is 
no  other  way  which  leads  to  the  exit  border  but  making  a  backward  motion  first.  Thus  in 
this  case,  the  backing-up  motion  planning  simulation  will  be  tried.  It  will  be  discussed  in 
the  subsection  e.  In  the  case  with  start  configuration  q^2^  the  image  of  start  configuration  on 
the  exit  edge  is  not  on  the  border.  Although  tracking  the  line  specified  by  the  exit 
configuration  will  result  a  collision  in  forward  simulation,  there  might  be  a  solution  other 
than  backward  motion.  This  solution  is  to  compute  an  alternative  reference  line  q2  as 
described  in  Section  C.l,  and  then  try  a  simulation  of  tracking  the  alternative  reference  line. 
If  the  simulation  makes  its  tracking  trajectory  a  perfect  convergence,  the  initial  motion  can 
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be  planned  to  track  q2  first  followed  by  tracking  the  line  of  exit  configuration  as  Figure 
5.16. 


Figure  5.15:  The  Cases  of  Forward  Forerunner  Simulation  with 
Collision  Detected 


If  the  simulation  with  alternative  reference  line  can  not  make  a  perfect 
convergence  trajectory,  the  backing-up  motion  planning  simulation  then  follows  to  seek  the 
solution. 


Figure  5.16:  An  Example  of  Alternative  Reference  Line  in  Solving 
Forward  Forerunner  Simulation  with  Collision  Problem. 


c.  The  Third  Planning  Simulation 

This  simulation  is  conducted  after  knowing  the  start  configuration  is  too 
close  to  the  exit  border  to  make  tracking  the  reference  line  converge.  It  takes  the  exit 
configuration  in  the  next  region  on  the  global  path  class  as  its  reference  line  for  the  first 
trial.  If  this  simulation  find  that  the  tracking  trajectory  is  non-collision  and  perfectly 
converges  to  the  reference  line,  the  simulation  terminates  and  the  motion  is  planned  to  track 
the  reference  line  as  illustrated  in  Figure  5.17. 


Figure  5.17:  The  Example  of  Initial  Motion  Planning  with  Reference 
Lines  in  the  Region  Next  to  the  Initial  Region. 

If  the  Third  Planning  Simulation  find  that  it  is  not  possible  to  have  a  perfect 
convergence  in  the  tracking  trajectory  with  q^  as  its  reference  line,  the  other  alternative 
reference  line  in  the  second  region  of  the  global  path  class  will  be  computed  and  the  Fourth 
Planning  Simulation  will  follow  to  try  to  find  the  solution. 

d.  The  Fourth  Planning  Simulation 

The  line  q^  will  be  computed  and  used  as  the  reference  line  in  this 
simulation.This  line  is  defined  in  the  region  next  to  the  initial  one.  Consulting  the  planning 
rule  in  mid-portion  motion  planning,  we  can  identify  the  reference  line  in  that  region  in 
mid-portion  motion  planning.  It  can  be  one  of  the  center  lines  or  the  hne  specified  by 
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entrance  configuration  or  exit  configuration.  Since  the  entrance  and  exit  border  of  the 
region  are  known,  the  position  on  the  line  ^4  =  (P4,  64,  0),  where  we  expect  the  line 
tracking  to  converge,  can  be  computed  as  described  in  Section  C.l. 

The  Fourth  Planning  Simulation  is  a  forward  forerunner  simulation.  It 
tracks  the  reference  line  from  start  configuration  q^.  If  it  outputs  a  trajectory  of  perfect 

convergence,  the  initial  region  motion  will  be  planned  to  track  the  reference  line  (74  until 
the  robot  reaches  P4.  Then  a  perpendicular  line  tracking  from  ^4  to  the  line  of  exit 
configuration  follows.  Figure  5.18  illustrates  this  planning.  If  no  such  a  trajectory  is  found, 
it  goes  to  performing  backing-up  motion  planning  simulation. 


Figure  5.18:  The  Example  of  Initial  Motion  Planning  with  Reference 
Lines  in  the  region  Next  to  the  Initial  Region. 


e.  The  Backing-up  Motion  Planning  Simulation 
When  a  single  forward  motion  is  not  possible  to  complete  the  initial  motion 
planning,  the  backing-up  motion  simulation  will  be  the  only  solution  to  this  problem.  The 
goal  this  simulation  is  either  the  virtual  robot  backs  up  to  the  orientation  of  the  forward 
reference  line  or  the  forward  simulation  achieves  a  perfect  convergence.  The  algorithms  for 
backing-up  motion  simulation  is  presented  in  Section  C  of  this  chapter.  This  simulation 


requires  two  predetermined  reference  lines.  One  is  for  backward  motion  and  another  one  is 
for  forward  motion.  Basically  the  reference  line  for  forward  motion  is  the  line  defined  by 
exit  configuration  in  initial  region  for  most  cases.  However,  it  can  be  a  line  which  is 
perpendicular  to  the  line  of  exit  border  as  referred  q2  in  Section  C.  The  simple  mle  for 
determining  forward  simulation  reference  line  is;  if  the  image  of  start  configuration  on  the 
exit  edge  is  not  on  the  border,  then  compute  a  line  ^2  for  the  reference  line.  Otherwise,  the 
reference  line  will  be  the  line  specified  by  the  exit  border.  For  backing-up  motion 
simulation,  the  reference  line  is  as  Eq  5.1  in  Section  C.l. 

The  smoothness  for  forward  forerunner  simulation  is  computed  as  Eq  5.4. 
For  backing-up  motion  simulation,  the  minimum  smoothness  as  Eq  5.3  will  be  applied  to 
make  the  backward  motion  achieve  its  goal  as  quickly  as  possible. 

After  the  reference  lines  and  smoothness  are  determined,  the  backing-up 
simulation  algorithm  with  forward  simulations  inside  of  it  can  perform  its  first  simulation. 
Once  the  goal  is  reached,  the  simulation  stops.  Let  q^,  q^,^,  q^gj  be  the  start  configuration, 
the  configuration  the  backing-up  simulation  stops  and  forward  reference  line  respectively. 
The  initial  motion  is  thereby  planned  as  follows:  (i)  backing-up  from  q^  to  q^,^.  (ii)  perform 
a  forward  motion  tracking  q^-g^.  Figure  5.19  demonstrates  an  example  of  initial  motion 
planning  with  single  backing-up  simulation.  In  the  example,  the  backing-up  simulation 
terminates  at  the  configuration  where  the  forward  forerunner  can  make  a  perfect 
convergence  in  tracking  the  forward  reference  line. 

In  the  case  that  the  initial  motion  planning  can  not  be  completed  by  single 
backing-up  simulation,  a  forward  simulation  and  a  backing-up  simulation  will  be  repeated 
in  order  until  the  goal  is  reached.  In  the  repeated  back  and  forth  simulation,  a  few  works 
will  be  processed  before  next  simulation  begins: 

•  The  last  configuration,  where  the  backing-up  or  forward  simulation  stops,  is 
taken  as  an  initial  configuration  for  next  simulation  and  its  curvature  is  reset  to 
zero.  This  is  necessary  and  possible  because  back  and  forth  motion  is  a 
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combination  of  many  discrete  motions.  The  robot’s  curvature  can  be 
discontinuous. 

♦  The  smoothness  for  the  forward  simulation  will  be  recomputed  according  to  its 
distance  to  the  reference  line. 

♦  The  reference  line  for  the  backing-up  simulation  will  be  recomputed  according 
to  the  new  start  configuration: 

There  is  a  possibility  that  neither  backing-up  nor  forward  simulation  can  be 
conducted.  In  this  case,  the  simulation  will  terminate  and  report  an  error. 


Figure  5.19:  An  Example  of  Backing-up  Motion  Simulation  in  the 
Initial  Motion  Planning. 

4.  Initial  Motion  Planning  Algorithms 

Some  notations  will  be  used  in  the  initial  motion  planning  algorithms.  They  are 
listed  as  follows: 

♦  q/.  Start  configuration; 

•  Qref  -  (Prep  ^refi  0):  Current  reference  line; 

*  Qrefi  ~  (Prefh  ^refb  0)^  the  reference  line  for  backing-up  simulation; 

*  QrefZ  -  ^ refly  ^refi'  0):  the  reference  line  for  forward  forerunner; 

•  Qj,  q2,  q^,  qp  the  lines  defined  in  Section  B.l; 


•  dy.  the  distance  from  current  configuration  to  the  reference  line; 

•  a:  current  smoothness; 

•  Th®  minimum  smoothness  as  defined  in  Eq  5.3; 

•  n^;  a  path  of  a  sequence  of  configurations  used  to  store  a  forward  trajectory; 

•  a  path  of  a  sequence  of  configurations  used  to  store  a  backing-up 
trajectory; 

•  backup:  a  flag  indicating  whether  backing-up  motion  is  needed; 

•  FWcollide:  a  flag  indicating  whether  a  collision  exists  in  forward  simulation; 

•  FWPerfectConverge:  a  flag  indicating  whether  forward  motion  converges 
perfectly; 

•  BWcollide:  a  flag  indicating  whether  a  collision  exists  in  backing-up 
simulation; 

•  BWpath[]:  a  structure  used  to  store  backing-up  paths; 

•  FWpath[]:  a  stmcture  used  to  store  forward  motion  paths. 

The  initial  motion  planning  algorithms  are  presented  as  the  followings: 

Algorithm  InitialMP  (q^,  path,  W) 

Input:  q^:  initial  configuration;  path:  a  global  path  class;  W:  a  world  model 
Output:  Motion  planning  data  structure 

(1)  backup  =  FALSE; 

(2)  FirstPlanning  ( q^,  path,  W ) 

(3)  if  (backup)  then  /*  back-up  motion  is  needed  */ 

(4)  BackUpPlanning(q^,path,W); 


Algorithm  FirstPlanning  (q^  path,  W) 

Input:  qy-  initial  configuration;  path:  a  global  path  class;  M:  a  world  model 
Output:  Motion  planning  data  structure 
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(1)  Qj-gf  =  qj;  I*  the  line  specified  by  exit  configuration  in  initial  region  */ 

(2)  compute  dy; 

(3)  a  =  max  ( o^in,  0.22  *  dy); 

(4)  ForwardSimulation  ( q^,  q^-^p  o,  W ); 

(5)  if  ( not  FW collide)  then  I*  no  collision  */ 

(6)  if  ( FWPerfectonverge )  then  /*  converge  perfectly*! 

(7)  Motion  Planning:  Tracking  q^^pfrom  q^  with  smoothness  a. 

(8)  else  /*  not  converge  before  pj  on  line  qj  */ 

(9)  ThirdPlanning  ( q^,  path,  W) ; 

(10)  else  /*  collision  in  initial  region  */ 

(11)  SecondPlanning  ( q^,  path,  W)  ; 


Algorithm  SecondPlanning  (q^,  path,  W) 

Input:  q.:  initial  configuration;  path:  a  global  path  class;  M:  a  world  model 
Output:  Motion  planning  data  structure  and  backup:  a  boolean  variable 

(1)  if  ( image  of  start  configuration  q^  is  on  exit  border )  then 

(2)  backup  =  TRUE; 

(3)  else 

Qref  =  compute  forward  reference  line  q2; 

(^)  ^  ~  ^min’ 

(6)  ForwardSimulation  (q^,  q^^p  ct,  W); 

(7)  if  (( not  FWcollide)  and  (FWPerfectConverge ) )  then 

(8)  Planning: 

(9)  1.  Tracking  q^^pfrom  q^  with  smoothness  a  until  reaches  q^^p 

(10)  2.  Tracking  q j  from  q^ep. 

(11)  else 

(12)  backup  =  TRUE; 


Algorithm  ThirdPlanning  (q^,  path,  W) 

Input:  q^;  initial  configuration;  path:  a  global  path  class;  W:  a  world  model 
Output:  Motion  planning  data  structure  and  backup:  a  boolean  variable 

( 1)  q^gj-  =  q^;  /*  qQ^ofthe  region  next  to  initial  region  *1 

(2)  compute  dy; 

(3)  o  =  max  ( 0.22  *  dy); 

(4)  Forwards imulation  ( q^,  q^^p  (5,W); 


(6)  if  (( not  FW collide )  and  ( FWPerfectConverge ) )  then 

(7)  Planning:  Tracking  q^gffrom  with  smoothness  o; 

(8)  else 

(9)  Fourthplanning  ( q^,  path,  W) ; 


Algorithm  FourthPlanning  (q^,  path,  W) 

Input:  q^:  initial  configuration;  path:  a  global  path  class;  W:  a  world  model 
Output:  Motion  planning  data  structure  and  backup:  a  boolean  variable 

(1)  q^-g^  —  compute  q^;  I*  reference  line  of  next  region  */ 

(2)  compute  rfy; 

(3)  G  =  max  ( 0^1^,  0.22  *  dy); 

(4)  ForwardSimulation  (q^,  q^gp  g,  W); 

(0)  if  (( not  FW  collide )  and  ( FWPerfectConverge ) )  then 

(7)  Planning: 

(8)  1.  Tracking  qggpfrom  q^  with  smoothness  g  until  reaches  p^gp; 

{9}  2.  Tracking  qj  from  q^^. ; 

(10)  else 

(11)  backup  =  TRUE; 


Algorithm  BackUpPlanning  (q^,path,  W) 

Input:  q^:  initial  configuration;  path:  a  global  path  class;  W:  a  world  model 
Output:  Motion  planning  data  structure 

(I)  if  ( image  ofq^  is  on  exit  border)  then 

(V  QrefZ  =  9l’ 

(3)  else 

(^)  9ref2  -  compute  forward  reference  line  q2; 

(5)  compute  dy  for  forward  forerunner  with  reference  line  q^g^  ; 

(6)  G  =  max  ( G^j„»  0.22  *  dy); 

(^)  Qrefl  -  (Ps’  ^ref2  +  0),' 

( 8 )  BackVpS imulation  ( q^ ,  q^gfj ,  q^-gp^ ,  G,  ; 

(9)  counter  =  1 ; 

(10)  B  Wpath[ counter ]  =  store 

(II)  if  ( FWPerfectConverge )  then 

(12)  FWpathl counter]  =  store 

(13)  while  (BW collide  and  not  done ) 
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(14)  =  last  configuration  in 

(15)  4new  ~  (Pnew>  ®new>  (^)’ 

(16)  ForwardSimulation  (q^g^  q^gfz,  Omin,  W); 

(17)  if  ( q„g^  =  last  configuration  in  Ylj^)  then  exit  and  report  error; 

(18)  if  (FWPerfectConverge )  then 

(19)  FWpathl counter]  =  store  Tlj^; 

(20)  done  =  TRUE; 

(21)  else 

(22)  q^igy^  =  last  configuration  in 

(23)  Qnew  ~  (Pnew  ^new  0)’ 

(24)  a  =  max  ( g^(„,  0.22  *  dy); 

(25)  drefl  ~  (Pnew  ^ref2  0)’ 

(26)  BackUpSimulation  (q^g^  q^gj-j,  q^g^,  c,  W); 

(27)  increment  counter; 

(28)  BWpathl counter]  =  store 

(29)  if  ( FWPerfectConverge )  then 

(30)  FWpathl  counter ]  =  store 

(31)  end  while ; 

(32)  Planning; 

(33)  while  ( counter  >0) 

(34)  backup  tracking  B  Wpath[ counter ] ; 

(35)  forward  tracking  FWpathl  counter]; 

(36)  decrement  backupPath; 

(37)  Tracking  qj; 


D.  FINAL  MOTION  PLANNING 

Let  the  path  FI  =  (Rj,  Bj . .  Rf^)  be  the  global  path  class  obtained  in  global  path 

planning  execution  after  a  mission  is  given,  where  n  >  1.  The  local  motion  planning  for  the 
final  region  and  probably  the  region  near  the  final  one  is  called /iwal  motion  planning. 
The  characteristics  of  the  robot’s  motion  in  the  final  region  of  the  path  is  that  the  robot  must 
stop  at  the  goal  configuration  specified  in  the  given  mission.  A  normal  path  tracking  motion 
maybe  suitable  for  some  cases  in  which  the  orientation  of  goal  configuration  has  small 
difference  compared  to  the  orientation  of  entrance  border  of  the  final  region.  In  many  other 
cases,  however,  the  difference  between  those  two  orientations  may  be  large.  Then  the 
simple  path  tracking  motion  may  not  be  able  to  fulfill  the  task  in  the  final  region.  One  of 


the  methods  for  solving  the  final  motion  planning  problem  is  proposed  as  Bidirectional 
Motion  Planning  [7]. 

As  we  notice  that  the  Bidirectional  Motion  Planning  is  used  to  generate  a  reference 
path  between  two  configurations.  In  the  case  of  final  motion  planning,  one  of  these  two 
configurations  is  the  goal  configuration  which  is  fixed  and  given  by  the  mission.  But 
another  configuration  is  unknown  untU  we  determine  where  the  Bidirectional  Motion 
Planning  commences  will  be  better.  In  order  to  make  a  better  or  possible  final  motion 
planning  using  the  Bidirectional  Motion  Planning  algorithm,  the  determination  of  the 
starting  configuration  is  important.  However,  this  still  remains  an  open  question. 

Another  important  task  in  the  final  motion  planning  is  to  plan  a  collision-free 
motion.  The  Bidirectional  Motion  Planning  is  an  excellent  approach  in  planning  a  path  to 
connect  two  configurations.  However,  it  is  provided  that  the  planning  will  be  performed  in 
an  obstacle-free  space.  As  we  know,  final  motion  planning  is  based  on  a  global  path  class 
which  relies  on  decomposed  regions.  That  means  the  obstacles  exit  not  only  in  the  real 
world,  but  also  in  the  world  model  we  are  working  on.  For  those  reasons,  the  Bidirectional 
Motion  Planning  is  considered  not  practical  in  this  dissertation. 

Local  motion  planning  is  to  plan  a  safe  and  symmetric  path  for  robot  to  follow.  The 
final  motion  planning  is  actually  the  mirror  of  the  initial  motion  planning  in  the  sense  of 
symmetry.  Therefore,  we  propose  that  the  final  motion  planning  be  performed  by  the 
approach  used  in  initial  motion  planning.  Only  by  doing  this,  can  the  entire  path  planned 
from  start  configuration  to  goal  configuration  be  symmetric  to  the  path  planned  reversely. 

To  apply  the  motion  planning  approach  used  in  initial  planning  to  final  motion 
planning,  the  algorithm  described  in  Section  C  can  be  adopted  with  a  minor  modification. 
First  of  aU,  the  goal  configuration  =  (p^,  0^,  0)  needs  to  be  replaced  as  Eq  5.5: 

Qg  =  (Pg,  9g+  7C,  0)  (Eq  5.5) 

That  is  to  reverse  the  orientation  of  the  goal  configuration.  The  same  operation  will  be 
applied  to  the  orientations  which  define  the  entrance  configurations,  ^jyj,  of  the  final  region 
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and  the  region  prior  to  it.  After  then  the  entrance  configiurations  wUl  be  rephrased  as  exit 
configurations.  So  as  to  the  entrance  borders  and  entrance  edges.  With  all  those  orientations 
and  names  reversed,  the  final  motion  planning  can  be  done  by  the  algorithm  presented 
below.  The  algorithms  of  all  other  subroutines  called  by  the  algorithm  FinalMP  will  be 
similar  to  the  algorithms  described  in  Section  C. 


Algorithm  FinalMP  (q^,  path,  W) 

Input:  q^:  goal  configuration;  path:  a  global  path  class;  W:  a  world  model 
Output:  Motion  planning  data  structure 

(1)  backup  =  FALSE; 

(2)  qg  =  reversed  q^; 

(^)  %ut-init  =  reversed  q^Jor  final  region; 

Q  out-next  -  reversed  q  in  for  the  region  prior  to  the  final  region  on  the  global  path; 

(5)  FirstPlanning  ( q^,  path,  W) 

(6)  if  (backup )  then  /*  back-up  motion  is  needed  */ 

(7)  BackUpPlanning(qg,path,W); 
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VI.  YAMABICO  HARDWARE  ARCHITECTURE 


This  chapter  introduces  the  hardware  structure  of  the  robot  —  Yamabico-11  which  is 
used  to  test  most  of  our  algorithms  experimentally. 

A.  OVERVIEW 

Yamabico-11  is  a  wheeled  untethered  indoor  mobile  robot  for  AI  and  robotics 
research.  It  has  been  developed  at  the  Naval  Postgraduate  School  over  the  last  several 
years.  However,  the  vehicle  is  a  result  of  Dr.  Yutaka  Kanayama's  long  history  of 
autonomous  robotics  research  at  the  University  of  Electro-Communications,  the  University 
of  Tsukuba,  Stanford  University,  and  the  University  of  California  at  Santa  Barbara  [24], 
[25].  Its  main  CPU  board  consists  of  the  SPARC  microprocessor  and  a  16  Mbyte  RAM 
storage  and  is  mounted  on  a  VME  bus.  Besides  that,  a  dual-axis  controller  for  two  motors 
and  two  shaft  encoders,  a  tailor-made  sonar  board,  and  a  serial  communication  board  are 
also  mounted  on  the  VME  bus.  One  lap-top  computer  is  used  for  a  real-time  input/output 
device.  The  size  is  60(W)  by  60(L)  by  70(H)  centimeters.  It  weighs  about  60  kilograms. 
The  differential  drive  kinematic  architecture  is  used  for  the  wheel  system.  Two  35  watts 
DC  motors  with  shaft  encoders  are  used  with  1/24  gear  boxes.  Twelve  40KHz  sonars  and 
one  CCD  camera  are  mounted  on  board.  Its  power  source  is  two  12  volt  car  batteries. 
When  object  code  is  downloaded  from  a  UNIX  system,  the  vehicle  operates  as  an 
untethered  (self-contained)  autonomous  robot.  [19].  Figure  6.1  illustrates  the  Yamabico-1 1 
hardware  architecture. 

B.  IV-SP ARC-33  CPU 

The  Ironies  IV-SP  ARC-33  is  a  single  processor,  VMEbus  Interface,  CPU  board.  It 
contains  a  25Mhz  SPARC  Integer  Unit,  a  Floating  Point  Unit,  and  a  Cache  Controller  and 
Memory  Management  Unit.  The  card  installed  in  Yamabico  has  64  Kbytes  of  cache,  and 
16  Mbytes  of  80ns  DRAM.  It  provides  two  RS-232  serial  I/O  ports,  two  programmable 
timers,  and  seven  user-definable  LEDs  [26]. 
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Figure  6.1:  Block  Diagram  of  Yamabico-11  Hardware  Architecture 
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1.  Address  Map 

The  Ironies  SPARC  board  contains  16  Mbytes  of  physical  memory,  yet  provides  32 
bit  addresses  (4  GBytes).  This  4  GByte  address  space  is  logically  divided  into  several 
regions.  The  three  most  important  regions  are  the  Local  DRAM,  Region  3,  and  Local  I/O 
(Figure  6.1). 


a.  Local  DRAM 

The  Local  DRAM  is  the  physical  memory  present  on  the  board,  and  is 
addressed  from  0x00000000  to  OxOOFFFFFF.  The  DRAM  array  is  organized  into  two-word 
(64-bit)  interleaved  banks  of  8  Mbytes  each.  Bidirectional  latching  data  buffers  direct  to 
and  from  the  two  banks.  The  DRAM  contains  the  SPARC  trap  table,  ISPARCmon  stack, 
uninitialized  data  segment  (bss),  Global  Work  Area,  and  all  the  target  program  segments. 
The  target  program  (application  program)  is  loaded  by  default  starting  at  location 
0x00018000.  From  this  lower  limit  up,  the  text  segment  is  first,  followed  by  the  initialized 
data  and  uninitialized  data  (bss).  The  region  for  target  program  stack  starts  from  the  address 
(DRAM  TOP  -  OxOOOOOOFF)  because  the  top  256  bytes  are  reserved  for  mailbox  interrupts. 
This  stack  is  extended  downward  from  the  top  of  this  region. 

b.  Region  3 

Region  3  starts  at  the  end  of  Region  2,  and  extends  to  the  bottom  of  the 
EPROM  space.  The  default  configuration  provides  addresses  from  OxFCOOOOOO  to 
OxFFOOOO,  however,  only  the  upper  boundary  is  fixed.  The  lower  boundary  may  be 
changed  by  writing  to  the  appropriate  register  as  defined  in  the  Ironies  manual.  MMLll 
currently  does  not  change  the  default  address  map,  but  does  provide  for  Region  3  to  be 
VMEbus  A16  addressable.  All  devices  on  the  VMEbus  are  addressed  from  Region  3. 
Addresses  are  obtained  by  adding  the  16-bit  base  address  of  a  specific  hardware  device  to 
the  Region  3  offset  of  OxFCOOOOOO.  This  includes  the  shaft  encoders,  quad  serial  boards, 
and  sonar  board. 
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OxFFFFFFFF 


c.  Local  HO 


The  Local  I/O  region  contains  the  addresses  for  the  registers  which  control 
operation  of  the  Ironies  SPARC  board.  They  are  addressed  from  OxFFFOOOOO  to 
OxFFFFFFFF.  Both  limits  are  fixed. 

2.  Registers 

There  are  two  sets  of  registers  which  control  the  CPU  board;  one  set  consists  of  8- 
bit  registers,  and  the  other  consists  of  16-bit  registers.  The  8-bit  registers  start  at  address 
OxFFCOOOOO,  and  the  16-bit  registers  start  at  address  OxFFDOOOO.  The  addresses, 
functions,  and  default  settings  are  clearly  defined  in  the  Ironies  users  manual. 

The  value  of  these  registers  many  be  changed  by  writing  directly  to  the  appropriate 
address,  or  by  using  one  of  the  two  library  functions  provided  with  the  CPU  board. 
Similarly,  their  values  can  be  read  directly  or  through  library  functions.The  current  version 
of  MMLll  uses  the  direct  write/read  method  in  order  to  avoid  additional  overhead 
associated  with  a  function  call. 

3.  Interrupt  Map 

The  Ironies  SPARC  CPU  board  emulates  a  680x0-style  interrupt-acknowledge 
cycle.  A  library  function,  mk_handler(),  assigns  a  user  defined  interrupt  handler  to  an 
appropriate  interrupt  vector.  All  interrupt  vectors  are  defined  in  the  Ironies  users  manual. 

The  syntax  for  this  function  is:  mk_handler  (vector_number,  interrupt_handler) 

4.  Internal  Interrupts 

Internal  Interrupts  are  those  generated  on  the  CPU  board.  The  two  most  important 
are  the  Timer  1  and  Timer  2  interrupts.  Timer  1  can  be  set  to  provide  interrupts  at  50,  100, 
or  1000  hz.  Currently,  MMLll  uses  Timer  1  to  provide  the  10ms  (100  Hz)  motion  control 
interrupt.  Timer  2  provides  a  broader  range  of  interrupts,  and  is  currently  unused.  The 
intermpt  vectors  for  Timer  1  and  2  are  defined  by  the  Local  Interrupt  Vector  Base  Register 
(0xFFFC0057). 
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5.  External  Interrupts 

External  Interrupts  are  those  generated  off  the  CPU  board.  The  most  important  are 
from  the  Quad  Serial  Boards,  and  the  Sonar  Board,  which  are  handled  through  the  7 
VMEbus  Interrupt  Request  lines.  Currently  the  VMEbus  cards  are  hardware  jumpered  to 
the  following  interrupts: 

IRQl:  Serial  Board  0,  ports  lA  and  IB  (which  are  unused),  and  the  #5  timer 
IRQ2:  Serial  Board  1,  ports  lA,  IB,  2A,  2B,  which  are  all  unused 
IRQ3:  Serial  Board  0,  ports  2A  and  2B  (host  and  old  console  connections) 

IRQ4:  Serial  Board  1,  #5  timer 
IRQ5:  Unused 
IRQ6:  No  connection 
IRQ7:  Abort  button 

Since  the  Serial  Board  ports  jumpered  to  IRQ2  are  unused,  this  request  line  is  being 
used  to  handle  interrupts  generated  by  the  Sonar  Board.  The  number  of  the  IRQ  does  not 
reflect  its  priority.  Priorities  are  set  by  modifying  the  appropriate  IRQ  Interrupt  Control 
Register  (0xFFFC0007-0xFFFC()01F).  Additionally,  the  VMEbus  cards  must  be  set  to 
assert  an  interrupt  vector  as  defined  in  registers  0xFFFC0087-0xFFFC009F. 

C.  SONARS 

The  sonar  system  mounted  on  Yamabico-1 1  consists  of  a  sonar  ranging  board  and 
a  sonar  array  consisting  of  twelve  Nippon  Ceramic  T40-16/R40-16  ultrasonic  range  finder 
emitter/receiver  pairs  arranged  around  the  robot’s  perimeter.  The  ranging  board  is  an 
Ommibyte  OB68K  VME  I/O  board  that  is  controlled  by  an  8748  microcontroller.  The 
sonar  board  is  a  separate  input/output  controller  that  makes  the  overall  sensor  process  more 
efficient  [21]. 

Currently,  Yamabico's  12  sonars  are  positioned  around  the  periphery  of  the  robot 
with  30  degree  increments  from  the  central  one  on  the  robot’s  front  edge.  The  actual 
positions  of  sonars  are  measured  from  the  center  of  the  robot  with  an  imaginary  Cartesian 


124 


Coordinate  system.  This  positioning  provides  the  most  comprehensive  sonar  coverage. 
Figure  6.3  illustrates  the  sonars’  configurations. 


X 


1.  Sonar  Control 

The  sonar  control  board  is  actually  a  daughtercard  which  rides  on  a  VME  bus 
mothercard.  The  mothercard  carries  address  decoders,  bus  drivers  and  interrupt  control 
circuitry  in  the  Bus  Interface  Module  (BIM). 

When  the  sonar  has  completed  a  ranging  cycle  an  interrupt  request  is  provided 
to  the  BIM.  The  BIM’s  control  register  holds  information  which  determines  whether  an 
interrupt  is  to  be  generated  or  not,  and  if  so  which  interrupt  level  is  to  be  generated. 
Presuming  an  interrupt  is  generated,  when  the  correct  acknowledgment  returns  on  the 
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address  lines  the  BIM’s  vector  register  provides  the  vector  table  entry  where  the  central 
processor  may  find  the  vector  to  the  interrupt  handler.  The  correct  interrupt  level,  the 
interrupt  enable  bit  and  interrupt  vector  are  loaded  to  the  BIM  during  software 
initialization. 

2.  Sonar  Grouping 

In  order  to  reduce  sampling  time  the  sonars  are  operated  in  logical  groups  of  four. 
The  sonars  of  a  logical  group  are  all  pulsed  simultaneously  and  thus  the  sampling  time  is 
reduced  by  a  factor  of  four  as  compared  to  individual  firing  of  the  sonars.  The  sonars  of 
each  logical  group  are  oriented  in  such  a  way  as  to: 

•  prevent  mutual  interference 

•  provide  a  “look”  in  all  four  directions  from  each  group 

•  present  a  similar  aspect  from  each  sonar  during  a  rotational  scan 

Thus,  logical  group  0  consists  of  sonars  0,  2,  5  and  7,  group  1  consists  of  sonars  1, 
3, 4  and  6;  group  2  consists  of  sonars  8, 9, 10  and  11;  and  group  3  is  a  “virtual”  group  which 
consists  of  four  permanent  test  values.  The  sonars  of  a  group  are  symmetric  about  the 
robot's  axis  of  rotation. 

In  addition  to  being  logically  grouped,  the  sonars  are  also  physically  grouped.  The 
physical  grouping  of  the  sonars  is  made  to  distribute  the  electrical  load  over  the  driver 
boards  evenly  and  thus  minimize  any  electrical  transients  associated  with  operation  of  the 
sonar.  The  physical  grouping  connects  sonars  0,  2,  8  and  11  to  driver/amplifier  board  1; 
sonars  4,  5,  6  and  7  to  board  2;  and  sonars  1,  3,  9  and  10  to  board  3.  The  reader  will  note 
that  pairs  of  sonars  from  logical  groups  are  assigned  to  physical  groups,  for  example,  sonars 
0  and  2  from  logical  group  0  are  assigned  to  physical  group  (driver/amplifier  board)  1. 


126 


VII.  MML-11  SOFTWARE  ARCHITECTURE 

A.  OVERVIEW 

The  Model-based  Mobile-robot  Language  (MML)  is  a  library  of  mobile  robot- 
oriented  functions  written  in  the  C  language  in  UNIX  environment  [25].  It  has  been 
implemented  on  the  autonomous  mobile  robot  Yamabico-1 1  at  UCSB  and  NFS.  Over  the 
years,  MML  has  been  improved  through  its  many  different  versions.  Currently,  we  are 
developing  its  eleventh  version  called  MML-11. 

From  the  robot  control  point  of  view,  MML-1 1  is  a  programmable  software  system 
for  mobile  robot  operation.  The  main  procedure  of  the  system  conducts  all  necessary 
initializations  for  both  hardware  and  software.  A  user  program  will  be  called  after  the 
initializations  are  done.  Besides  the  main  procedure,  MML-11  mainly  consists  of  two 
subsystems: 

•  Motion  Control  Subsystem  and 

•  Sonar  Control  Subsystem 

A  user  application  program  is  required  for  the  robot’s  operation  using  this  software. 
For  user  application  programming  convenience,  the  system  provides  a  set  of  well-defined 
functions  called  user  functions  as  interface  between  the  user  and  the  system.  The  user 
functions  are  categorized  into  four  modules: 

•  Operating  System  Module 

•  Motion  Planning  Module 

•  Motion  Control  Module 

•  Sonar  Control  Module 

1.  System  Architecture 

This  software  is  developed  with  a  special  architecture  which  incorporates 
sequential  structure  and  interrupt-driven  structure.  The  system  initialization  and  the  user 
application  program  will  be  basically  executed  sequentially  in  the  main  procedure  of  the 
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system.  Alternatively,  the  separate  subsystems,  i.e.  Motion  Control  Subsystem  and  Sonar 
Control  Subsystem,  are  periodically  called  for  execution  via  interrupt  requests  for  the 
required  motion  control  and/or  sonar  control  operation.  Figure  7.1  illustrates  the  MML-11 
software  conceptual  architecture. 


Function 

Support 

Sequential 

execution 

Interrupting 

Request 


Figure  7.1:  MML-11  Software  Conceptual  Architecture. 


2.  Interrupt-driven  Subsystems 

There  are  three  primary  tasks  that  may  be  mnning  at  any  given  time.  The  highest 
priority  task  is  motion  control  subsystem,  which  performs  all  motion  control  computations 
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and  in  turn  translates  them  into  low-level  wheels  control.  This  subsystem  is  designed  to 
interrupt  other  tasks  in  the  frequency  of  10  milliseconds.  The  next  highest  priority  is  the 
sonar  control  subsystem,  which  processes  all  incoming  sonar  returns  and  generates  line 
segments  from  individual  sonar  returns  from  obstacles  if  required.  The  sonar  control 
subsystem  issues  interrupt  request  in  the  frequency  of  50  milliseconds.  The  lowest  level 
priority,  but  basic,  task  is  the  user  program.  This  part  of  the  system  feeds  both  immediate 
and  sequential  commands  to  the  motion  control  subsystem  through  a  command  queue.  All 
higher  priority  tasks  interrupt  the  tasks  with  lower  priorities  to  gain  the  CPU  control.  The 
design  of  MML-11  subsystems  will  be  described  in  following  sections. 

3.  Real  Time  Operating  System 

The  Yamabico-11  onboard  CPU,  IV-SPARC  33,  provides  no  standard  operating 
system  functions  but  a  small  set  of  libraries  for  console  I/O.  All  other  operating  system 
primitives,  such  as  interrupt  handling,  memory  management,  data  formatting  and  logging 
have  to  be  provided  by  the  MML  system.  The  detailed  design  of  real  time  operating  system 
for  robotic  system  will  be  presented  in  Chapter  VOL 

4.  User  Program 

In  this  software,  the  robot’s  motion  is  instructed  by  the  user  program,  which  sends 
commands  to  the  motion  control  system  and/or  sonar  control  system.  However,  motion 
planning  and  control  specific  concepts  are  hidden  from  the  user.  Only  those  defined  as  user 
functions  are  allowed  to  be  used  in  user  program.  Sonar  data  is  available  to  the  user  in  either 
a  raw  or  processed  format  via  user  sonar  functions.  Appendix  C  gives  an  user  program 
example.  The  MML-1 1  user  function  specifications  will  be  described  in  Chapter  IX. 

B.  MOTION  CONTROL  ARCHITECTURE 

The  motion  control  must  be  performed  in  a  short  period  repeatedly.  It  is  difficult  to 
impose  this  control  in  user’s  program.  As  we  design  intermpt-driven  software  system,  the 
foreground  job  and  background  job  concepts  are  introduced  into  MML-11  motion  control 
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software.  In  MML-1 1,  motion  control  mechanism  is  designed  in  the  way  that  the  execution 
of  user  program  is  somewhat  separated  from  motion  control.  This  allows  users  being  able 
to  program  their  applications  by  using  simple  functions.  The  user  program  is  considered  the 
foreground  process  which  sends  either  immediate  or  sequential  commands  to  the  system. 
The  robot  motion  control  task  conducted  by  motion  control  subsystem  is  considered  the 
background  process  which  performs  motion  control  to  achieve  the  motion  instruction  it 
gains  control  at  a  frequency  of  10  ms.  The  immediate  commands  in  the  user  program  will 
be  executed  immediately,  while  the  sequential  commands  will  be  enqueued  to  a  buffer 
called  the  instruction  buffer  waiting  for  execution  sequentially.  The  motion  control 
subsystem  fetches  an  instruction  sequentially.  When  the  execution  of  one  instruction  is 
finished,  the  control  subsystem  picks  and  executes  another  instruction  from  the  buffer  until 
the  buffer  is  empty.  The  motion  control  architecture  is  illustrated  in  Figure  7.2. 


Figure  7.2:  MML-11  Motion  Control  Software  Architecture. 
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C.  MOTION  CONTROL  SUBSYSTEM 


Motion  control  subsystem,  named  MotionSysControI,  is  the  foreground  process  of 
the  entire  system.  It  is  designed  to  compute  all  data  necessary  for  motion  control  by 
interrupting  system  main  procedure  (or  user  program)  every  10  milliseconds.  When  the 
interrupt  request  is  granted,  this  subsystem  gains  the  control  of  CPU.  It  actually  acts  as  an 
interrupt  service  routine.  This  section  presents  the  stmcture  of  MotionSysControI 
subsystem  and  discusses  how  the  robot  decides  its  transitioning  dynamically. 

I.  System  Structure 

MotionSysControI  performs  following  computations  for  the  robot  motion  control  in 
order  to  accomplish  its  mission. 

•  Measure  the  distance  traveled,  A  s,  in  a  cycle  by  reading  robot’s  left  and  right 
shaft  encoders. 

•  Compute  the  orientation  changed  A6 

•  Localize  current  configuration  q. 

•  Compute  commanded  linear  and  rotational  velocity,  Vl,  V^^,,  for  next  cycle 

•  Translate  commanded  velocity  into  control  signals,  PWM,  for  driving  motors 

•  Transition  point  simulation  to  decide  whether  to  read  next  instmction 

The  block  diagram  in  Figure  7.3  illustrates  motion  control  subsystem  structure.  By 
reading  robot’s  left  and  right  shaft  encoders,  the  process  can  measiue  the  distance  traveled. 
Computations  of  distance  traveled  and  orientation  changed  are  done  in  order  by  a  module 
with  outputs  A  s  and  A0.  These  data  will  be  used  by  localization  module  to  compute  robot’s 
current  configuration.  The  current  configuration  q  is  needed  for  motion  rule  module  to 
compute  commanded  linear  and  rotational  wheel  velocities,  Vl,  V(^,  for  next  cycle.  These 
velocities  are  translated  in  left  and  right  PWMs  as  signals  to  drive  corresponding  motors. 
The  last  step  in  MotionSysControI  is  to  determine  whether  or  not  to  start  transitioning  to 
next  path.  This  is  done  by  a  forerunner  simulation  in  real  time.  If  it  decides  to  transition, 
the  next  motion  command  in  the  instruction  buffer  will  be  read  and  followed. 


Foreground  Process 


Figure  7.3:  MML-11  Motion  Control  Subsystem  Structure. 


2.  Transition  from  One  Path  to  Another 

The  last  step  in  MotionSysControl  is  to  determine  whether  or  not  to  start 
transitioning  to  next  path.  In  MML-11  software  system,  the  robot’s  motion  can  be 
controlled  by  tracking  successive  path  segments  using  path  tracking  algorithm.  The 
possible  path  segments  are  straight  line  and  circular  arcs.  In  order  to  smooth  motion  and  to 
ensure  safety,  the  robot  must  transition  from  one  path  to  another  at  the  right  time  and  right 
position.  The  position  where  robot  begins  to  transfer  from  the  current  path  to  the  successive 
one  is  called  leaving  point.  The  transition  point  is  defined  as  the  last  leaving  point  on  the 
current  path  segment  such  that  the  robot’s  motion  trajectory  does  not  intersect  the  second 
path  segment.  The  distance  between  the  transition  point  and  the  intersection  of  two 
successive  paths  is  called  transitioning  distance  Figure  7.4  shows  the  different  trajectories 
of  a  robot  transitioning  from  a  path  pi  to  p2  at  different  leaving  points.  The  Figure  7.4  (a) 
is  an  early  leaving  point  which  makes  the  trajectory  longer  to  converge  to  path  p2.  In  Figure 
7.4  (b),  the  robot  transitions  to  path  p2  at  proper  leaving  point.  Figure  7.4  (c)  shows  a  late 
leaving  point  making  the  trajectory  intersect  path  p2. 


Figure  7.4:  The  Different  Motion  Trajectories 


133 


Therefore,  transitioning  from  one  path  to  another  at  the  right  time  and  right  position 
is  very  much  important  in  motion  control.  In  this  section,  we  introduce  a  new  method  to 
efficiently  compute  a  transition  point  for  robot  transition  control. 

a.  Real  Time  Dynamic  Transition  Control 

In  normal  transition  control,  the  transition  point  can  be  approximately 
calculated  in  foreground  process  when  more  than  one  path  segments  are  present  in  user 
program.  For  instance  for  transitioning  from  a  path  segment  to  a  perpendicular  path 
segment,  the  transition  distance  is  about  twice  as  much  as  the  given  smoothness  a.  Its 
transition  point  can  be  easily  calculated  according  to  this  distance.  After  the  transition  point 
is  obtained,  the  transition  can  be  controlled  by  checking  robot’s  current  position  against  the 
transition  point.  When  the  robot  reaches  the  transition  point,  it  transitions  to  track  the  next 
path  segment.  Knowing  transition  distance,  the  transition  point  calculation  is  not  too  hard 
in  this  case.  However,  the  transition  distance  is  not  fixed  in  other  cases.  On  the  contrary,  it 
varies  from  case  to  case  depending  on  the  some  other  factors,  e.  g.  orientation  difference  of 
two  path  segment,  curvature  limitations,  velocity  and  so  forth.  This  makes  transition  point 
calculation  inaccurate.  It  requires  complex  functions  or  a  large  data  table  to  properly 
determine  the  correct  transitioning  position. 

For  more  flexible  transition  control,  we  propose  a  new  method  to  determine 
when  to  transition  dynamically  without  bearing  heavy  cost.  The  method  is  to  use  the 
forerunner  simulation  in  real  time.  A  forerunner  is  a  virtual  robot  which  is  located  in 
certain  distance  ahead  of  the  real  robot  to  simulate  the  robot’s  future  trajectory.  If  the 
forerunner’s  trajectory  converges  to  the  reference  path,  another  new  forerunner  is  created 
based  on  robot’s  new  configuration.  These  forerunner  simulations  keep  running  until  the 
forerunner’s  trajectory  intersects  the  reference  path.  When  intersecting  happens,  it  implies 
that  transitioning  from  the  initial  position  of  this  forerunner  might  be  too  late  for  a  smooth 
motion.  The  correct  transition  point  must  be  earlier  than  the  last  one.  Therefore,  we  take 
middle  position  of  the  last  two  forerunners’  initial  position  as  the  transition  point. 
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Figure  7.5  illustrates  forerunner  simulation  operation.  In  the  figure,  pi 
represents  current  path  and  p2  represents  successive  reference  path.  When  the  real  robot  is 
in  configuration  vl,  the  forerunner,  with  distance  S  ahead  of  robot’s  current  configuration, 
in  configuration  fl  is  created.  The  forerunner  simulates  the  robot’s  trajectory  assuming  that 
f  1  is  the  transition  point.  When  the  first  forerunner  finished  its  job,  the  robot  had  moved  to 
configuration  v2  in  Figure  7.5  (b).  Since  the  trajectory  of  forerunner  in  f  1  converges  to  path 
p2,  the  second  forerunner  in  f2  is  created  with  the  same  manner.  The  simulation  of 
forerunner  in  f2  found  that  its  trajectory  intersects  path  p2.  Thus  the  simulation  stops  and 
the  transition  point  is  computed  as  the  point  Tp  in  Figure  7.5(b). 


pi  pi 


Figure  7.5:  The  Forerunner  Simulation. 

b.  Fast  Convergence  Detection 

In  straight  line  tracking  simulation,  if  the  trajectory  intersects  the  reference 
line,  it  can  be  identified  easily  by  transferring  the  reference  line  to  X  axis  (it  means  the  line 
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is  y=0)  and  checking  Y  coordinate  of  the  trajectory.  When  Y  coordinate  becomes  negative, 
it  intersects  the  reference  line.  However,  the  simulation  takes  significantly  longer  time  to 
identify  the  convergency  situation  if  it  is  done  with  this  method.  Therefore  a  faster  way  to 
determine  convergency  of  simulation  is  needed  in  forerunner  simulation. 

In  this  section,  we  are  introducing  a  fast  way  to  check  the  convergence 
situation  in  the  line  tracking.  We  define  shadow,  denoted  as  the  distance  between  and 
P2,  where  is  the  image  of  a  configuration  q  =  (p,  6,  k)  on  the  simulated  trajectory,  and 
P2  is  the  intersection  of  reference  path  and  the  tangent  line  of  the  trajectory  at  configuration 
q  (see  Figure  7.6). 


Figure  7.6:  The  Shadow  of  a  Trajectory 


It’s  obvious  that  the  shadow  ^  is  geometrically  related  to  the  component  of 
configuration  q  as  equation  7.1. 


tan  6 


(Eq7.1) 


We  define  the  normalized  shadow,  denoted  I*,  as  equation  1.2,  where  a  is  the  given 
smoothness. 
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(Eq7.2) 


F*  =  i 

^  o 

We  observed  the  normalized  shadows  in  the  simulation,  and  found  that  the 
normalized  shadow  reaches  a  maximum  value  in  all  cases.  Figure  7.7.(a)  shows  the 
trajectory  of  tracking  the  line  oiq  =  ((0,0),0,0)  with  initial  configuration  =  ((0,10), 
-  7C 12,  0).  Figure  7.7. (b)  shows  it  positive  normalized  shadows  vs.  trajectory  length.  The 
maximum  normalized  shadow  is  ^*  =  0.18  when  the  trajectory  intersects  the  reference  line. 
Another  simulation  result  is  shown  in  Figure  7.8.  Figure  6.8(a)  shows  the  trajectory  of 
tracking  the  line  of  q  =  ((0,  0),  0,  0)  with  initial  configuration  =  ((0,  22),  -  %  12,  0). 
Figure  7.8(b)  shows  it  positive  normalized  shadows  vs.  trajectory  length.  The  maximum 
normalized  shadow  in  this  case  is  =  1.17. 


Figure  7.7:  Positive  Normalized  Shadow  vs.  Trajectory  Length  I 
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Figure  7.8:  Positive  Normalized  Shadow  vs.  Trajectory  Length  n 


The  simulation  results  reveal  an  interesting  phenomenon,  in  which  ^*inax  < 
0.9  if  the  trajectory  crosses  the  reference  path  and  if  the  trajectory  converges  to  the 
reference  path,  ^*jnax  -  The  table  7.1  shows  the  experimental  data  with  initial 
conditions  given  as  follows. 

•  Initial  configuration  ql  =  (p,  0,  k)  =  ((0,  -%I2,  0) 

•  Reference  path  q2  =  (p,  0,  k)  =  ((0,  0),  0,  0) 

•  Distance  constant,  <5=  10.0 

•  Step  size.  As  =  0.2 


Table  7.1:  Max  Shadow  of  Different  Initial  Configuration 


yO 

10 

18 

19 

20 

22 

25 

^*max 

0.179 

0.738 

0.839 

0.946 

1.152 

1.362 

conver- 

gency 

crossing 

crossing 

crossing 

converging 

converging 
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The  data  in  Table  7.1  shows  the  experimental  results  for  some  fixed  initial 
condition.  This  phenomenon,  however,  is  consistent  with  other  given  initial  configuration 
and  smoothness.  Therefore,  the  fast  way  to  determine  whether  a  path  tracking  converges  to 
its  reference  line  is  by  checking  the  normalized  shadow  as: 

*  converge  if 

*  intersect  if  ^*niax<0.9 

The  fast  converge  detection  method  is  especially  valuable  in  real  time 
dynamic  transition  control  because  the  transition  point  can  be  determined  earlier  so  that  the 
transition  motion  can  be  much  smoother.  This  method  has  been  successfully  applied  to 
MML-1 1  which  makes  the  software  more  flexible. 

D.  SUMMARY 

MML-1 1  is  the  newest  version  in  MML  development  history.  One  of  the  important 
features  of  this  new  version  is  that  it  is  written  in  ANSI  C,  yet  is  designed  with  an  object 
oriented  perspective.  Individual  source  files  are  similar  to  C++  classes  by  way  of  their  data 
encapsulation  and  initialization.  Each  source  file  encapsulates  all  local  data  statically,  and 
the  only  means  of  accessing  the  data  from  outside  the  file  is  through  a  well-defined  set  of 
interface  functions  [19].  This  feature  secures  the  important  control  data  from  being 
accessed  or  modified  illegally  resulting  unexpected  behavior.  On  software  engineering 
point  of  view,  this  is  especially  helpful  in  the  long  term  system  development  and 
maintenance. 
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VIII.  ROBOT  REAL  TIME  OPERATING  SYSTEM  DESIGN 

A.  INTRODUCTION 

Operating  systems  are  primarily  resource  managers.  The  main  resource  they 
manage  is  computer  hardware  in  the  form  of  processors,  storage,  input/output  devices, 
communication  devices  and  data.  For  a  robotic  system,  the  computing  device  is  normally 
a  commercial  product  in  which  the  basic  operating  system  embedded.  For  instance, 
Yamabico-11  is  currently  equipped  with  an  Ironies  rV-SPARC-33  CPU  which  is  a  single 
processor  [26].  It  provides  general  computing  capabilities.  In  order  to  support  robot  control 
in  various  aspects,  a  suitable  operating  system  needs  to  be  developed.  In  this  chapter  we 
address  the  design  of  real  time  operating  system  for  the  robot  focusing  on  tasks  scheduling 
and  dynamic  memory  management. 

The  design  of  robotic  operating  system  mainly  involves  the  robot  hardware  control. 
In  what  manner  the  attached  hardware  is  to  be  controlled  is  the  guideline  of  such  a  design. 
Robot  motion  control  is  the  central  issue  of  robotic  system.  How  the  operating  system  is 
designed  to  handle  motion  control  is  the  most  important  of  concerns.  While  the  motion 
control  is  been  carrying  out,  gathering  surrounding  information  from  the  desired  sensors 
for  navigation  is  a  natural  operation.  The  robot’s  motion  must  react  to  the  senors 
information  in  most  cases.  For  a  single  processor  robotic  system  as  Yamabico-11,  the 
operating  system  is  designed  to  determine  when  and  how  each  of  control  routines  takes 
over  the  CPU.  Obviously,  a  multitasking  operating  system  is  needed  for  a  robotic  system 
to  handle  those  controls.  The  multitasking  operation  is  made  possible  by  implementing  an 
interrupt  mechanism  for  the  tasks.  In  addition  to  the  tasks  scheduling,  dynamic  memory 
management  is  another  issue  to  be  solved  in  Yamabico-1 1. 

B.  MULTITASKING  OPERATING  SYSTEM  DESIGN 

In  order  to  properly  design  an  interrupt-driven  operating  system  for  Yamabico-11, 
the  interrupt  mechanism  of  rV-SPARC-33  board  must  be  understood.  The  IV-SPARC-33 
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board  uses  seven  of  15  SPARC  asynchronous  traps  to  emulate  680x0  interrupt  level  1 
through  7  exceptions.  The  remaining  eight  asynchronous  traps  are  undefined  and  are  not 
presented  to  the  SPARC  processor.  Table  8.1  lists  the  traps  recognized  by  the  IV-SPARC- 
33  board. 


Table  8.1:  SPARC/680X0  Interrupt  Level  Equivalence 


SPARC  Interrupt  Level 

680x0  Intermpt 

lACK  Address 

1 

1 

0xFFFFFFF3 

2 

2 

0xFFFFFFE7 

3 

3 

0xFFFFFFF7 

4 

4 

OxFFFFFFEB 

5 

5 

OxFFFFFFFB 

6 

6 

OxFFFFFFEF 

7-14 

Spurious  Interrupt 

Undefined 

15 

7  (NMI) 

OxFFFFFFFF 

Traps  are  controlled  by  several  registers  in  SPARC  board  depending  on  the  trap 
types.  The  interrupting  traps  are  controlled  by  a  combination  of  the  Processor  Intermpt 
Level  (PIL)  field  and  Trap  Enable  (ET)  field  of  Processor  Status  Register  (PSR).  The  ET 
field  in  the  PSR  must  be  1  for  traps  to  occur  normally.  While  ET  =  1,  the  Integer  Unit  (lU) 
prioritizes  the  outstanding  exceptions  and  interrupt  requests  according  to  their  priority 
ranks.  For  interrupt  requests,  the  higher  interrupt  level  possesses  higher  priority,  e.g. 
interrupt  level  2  has  greater  priority  than  interrupt  1,  and  so  on.  When  an  interrupt  request 
occurs,  the  lU  compares  the  Interrupt  Request  Level  (IRL)  against  the  PIL.  If  IRL  >  PIL  or 
IRL  =  15  (Non-Maskable  Interrupt,  NMI),  the  processor  takes  the  interrupt  request  trap  and 
the  control  goes  to  that  interrupt  service  routine  [27]. 
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Although  the  SPARC  architecture  specifies  15  interrupt  levels,  the  onboard  logic  is 
capable  of  requesting  only  interrupt  levels  1  through  6  and  15.  Therefore,  we  must  be  aware 
of  the  interrupt-acknowledgment  precautions  that  the  interrupt  level  7  through  level  14  are 
not  defined  in  SPARC  board  and  interrupt  level  15  (NMI)  is  a  special  case  which  requires 
a  special  handling.  To  avoid  unexpected  results,  when  we  design  interrupt-driven  operating 
system,  level  7  to  level  14  should  not  be  used  in  any  case  and  the  use  of  interrupt  level  15 
must  be  very  carefully  handled. 

1.  System  Design  Considerations 

For  a  real-time  robot  system,  the  robot’s  motion  and  sensing  devices  operation 
should  be  controlled  by  the  software  at  all  time  until  its  mission  is  accomplished.  This 
implies  that  the  interrupts  of  motion  control  system  and  sonar  control  system  of  Yamabico- 
11  should  occur  periodically.  Thus,  the  determination  of  interrupt  frequencies  is  an 
important  part  of  the  design.  Another  aspect  of  the  design  consideration  is  which  task 
should  possess  higher  priority  so  that  it  is  allowed  to  interrupt  other  less  important  tasks 
and  preventing  being  interrupted  by  them.  The  hardware  interface  which  is  available  for  the 
implementation  of  interrupt  design  should  also  be  selected. 

With  these  requirements,  the  frame  work  of  multitasking  operating  system  design 
is  clarified.  The  system  design  may  involve  several  decisions  and  implementations 
including: 

•  Priorities  and  frequencies  assignment  decisions; 

•  Selection  of  available  interfaces; 

•  Interrupt  handling  service  routines  design  and 

•  Installation  of  interrupt  handler. 

2.  Interrupt  Level  and  Frequency  Assignment 

Higher  priority  tasks  are  allowed  to  interrupt  one  or  more  lower  priority  tasks  when 
required.  Therefore,  tasks  should  be  assigned  an  appropriate  priority  depending  upon  their 
relative  importances  when  designing  an  interrupt-driven  operating  system  for  a  robotic 
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system.The  higher  the  interrupt  level  is,  the  higher  the  priority  of  the  associated  task  will 
be.  Since  the  motion  control  is  the  most  important  task  compared  with  other  tasks,  it  should 
be  assigned  with  the  highest  priority  among  the  asynchronous  trap  level.  The  sonar  control 
task  can  be  assigned  with  other  lower  priority  level.  As  Table  10.1  indicates,  the  highest 
interrupt  level  in  SPARC  board  is  Level  6  and  the  lowest  level  is  1 .  We  design  the  operating 
system  of  Yamabico-11  with  following  priorities  assignments: 

•  Motion  control  system:  interrupt  level  6. 

♦  Sonar  control  system:  interrupt  level  3. 

This  design  allows  control  of  future  sensing  devices  like  vision  system  to  be 
inserted  between  those  levels  according  to  their  relative  importance.  The  Figure  8.1 
illustrates  the  interrupts  which  may  occur  in  the  MML-11. 


In  addition  to  the  priority  assignment,  the  frequencies  of  interrupts  also  needs  to  be 
determined  in  the  design.  For  more  precise  control  of  robot’s  motion,  motion  control 
system  interrupts  should  occur  over  short  periods,  while  still  allowing  other  task  to  regain 
control.  Thus,  we  design  the  frequency  for  motion  system  control  to  be  100  Flz,  and  for 
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sonar  system  control  20  Hz.  This  means  the  motion  system  control  interrupt  occiu:s  every 
10  milliseconds,  while  the  sonar  system  control  interrupt  occurs  every  50  milliseconds. 
And  that  the  motion  system  control  task  will  interrupt  the  sonar  system  control.  In  our 
estimation,  the  motion  system  control  task  runs  for  approximately  400  microseconds  (0.4 
ms)  and  sonar  control  system  task  runs  for  approximately  3200  microseconds  (3.2  ms). 

3.  Selection  of  Available  Interface 

The  programmed  interrupts  are  made  possible  by  specifying  desired  interrupts 
using  onboard  registers  as  an  interface.  Some  of  the  unused  board  configuration  registers 
are  chosen  for  our  interrupts  design  as  follows: 

a.  Registers  for  Motion  System  Control 

We  use  Timer  1  Interrupt  for  the  interface  to  enforce  motion  system  control. 
The  Timer  1  can  be  set  to  interrupt  at  rates  of  50  Hz,  100  Hz,  or  1000  Hz.  This  interrupt  is 
controlled  by: 

•  Timer  1  Interrupt  Control  Register  (address  0xFFFC002B) 

•  Local  Interrupt  Vector  Base  Register  (address  0xFFFC0057) 

•  Slave  Select  0  /  Timer  1  Control  Register  (address  0xFFFC00C3) 

The  vector  return  to  the  CPU  when  it  issues  an  interrupt  acknowledge  in 
response  to  the  Timer  1  interrupt  is  0x4A.  This  vector  indicates  the  location  of  interrupt 
handler  vector  table  which  will  be  replaced  by  the  address  of  the  interrupt  service  routine. 
Thus  using  a  variable,  Motionint Vector,  to  represent  the  vector  location,  we  have: 

MotionIntVector  =  0x4  A; 

b.  Registers  for  Sonar  System  Control 

For  sonar  system  control  the  VMEbus  IRQ2*  Interrupt  Control  Register 
(address  OxFFFCOOB)  is  selected  because  the  sonar  card  is  connected  to  VMEbus  line 
IRQ2*.  The  vector  associated  to  VMEbus  IRQ2*  Interrupt  Control  Register  is  identified  as 
0xA2.  Using  another  variable,  SonarIntVector,  to  represent  this  vector  location,  we  have: 

SonarIntVector  =  0xA2; 
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4,  Interrupt  Service  Routine 

The  interrupt  service  routines  can  be  executed  upon  receipt  of  the  vector  associated 
with  a  specific  interrupt.  The  content  of  an  interrupt  service  routine  reflects  its  desired 
functionality.  For  motion  system  control,  the  routine,  named  MotionSysControI,  is 
designed  firstly  to  read  the  shaft  encoders  and  computes  the  vehicle’s  odometry 
configuration  estimate.  Then  the  desired  curvature  and  velocity  are  calculated.  This 
information  is  used  to  determine  the  necessary  pulse  width  modulation  commands  for 
controlling  the  left  and  right  wheel  drive  motors.  For  the  sonar  system  control,  the  routine, 
named  SonarSysControl,  calculates  the  range  of  enabled  sonar  from  its  returned  value. 
Then  write  it  to  the  memory  in  order  to  be  used  as  required.  The  detailed  design  of  routines 
is  beyond  this  chapter. 

5.  Interrupt  Handler  Installation 

The  steps  required  to  install  an  interrupt  service  routine  are: 

•  Set  up  the  required  interrupt  control  registers 

♦  Assign  interrupt  service  routine  vector  location. 

The  design  of  motion  control  interrupt  and  sonar  control  interrupt  foUow  these 
steps.  They  are  described  this  the  following  two  sections. 

a.  Motion  System  Control  Interrupt  Handler  Installation 
Firstly  we  set  up  the  Timer  1  Interrupt  Control  Register  (0xFFFCO02B) 
which  specify  the  interrupt  level.  The  bit  7  of  this  register  is  used  to  enable  the  interrupt 
associated  with  the  timer.  Its  value  0  indicates  the  interrupt  is  enabled.  The  bit  6:4  are 
reserved.  Their  values  should  be  kept  as  they  are  in  default  setting.  The  bit  3  is  read  only, 
and  a  write  on  this  bit  has  no  effect.  The  bit  2:0  specify  the  interrupt  level  seen  by  the  CPU. 
Setting  this  register  to  enable  the  motion  system  control  interrupt  in  level  6  makes  the 
contain  of  the  register  as  [001101 10].  This  setting  can  be  done  by  using  bit-wise  operation 
based  on  default  value  of  the  register.  Two  additional  bit-wise  variables  are  set  as 
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InterruptMask  =  0x30  and  InterruptLevelO  =  0x06  to  accomplish  the  logic  operation  as 
Figure  8.2. 
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Figure  8.2:  Setting  Timer  1  Interrupt  Control  Register 

Then  set  up  the  Slave  Select  0  /  Timer  1  Control  Register  (OxFFFCOOCS) 


which  is  mainly  used  to  specify  the  interrupt  frequency.  The  setting  is  done  by  logic 
operation  on  register  default  setting  and  a  bit-wise  variable  EnablelOOHz  =  0x80  as  Figure 
8.3.  The  final  contain  of  this  register  is  [11010010]  where  the  value  11  in  bits  7  and  6  set 
the  frequency  to  100  Hz. 
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Figure  8.3:  Setting  Slave  Select  0  /  Timer  1  Control  Register 


After  those  registers  are  set  up,  the  interrupts  are  ready  to  run.  Then  we  can 
install  the  motion  system  control  interrupt  service  routine  address  into  the  corresponding 
interrupt  service  routine  vector  location,  using  the  mk_handler()  library  function  as  below: 
nik_handler(MotionIntVector,  *MotionSysControl) ; 

MotionIntVector:  Location  in  interrupt  handler  vector  table. 

*MotionSysContro:  Function  pointer  to  be  placed  into  handler  vector  table; 
b.  Sonar  System  Control  Interrupt  Handler  Installation 
Sonar  system  control  interrupt  frequency  is  set  by  firmware  on  the  sonar 
card.  The  implementation  of  sonar  interrupt  involves  setting  the  VMEbus  IRQ*  interrupt 
Control  Register  only.  In  eight-bit  width  register,  the  bit  7  is  used  to  enable  the  interrupt. 
Its  value  0  indicates  the  interrupt  is  enabled.  The  bits  2:0  specify  the  interrupt  level  seen  by 
the  CPU.  The  rest  of  bits  are  reserved  and  should  be  kept  as  they  are  in  default  setting.  To 
set  the  sonar  system  control  interrupt  to  level  3  and  enable  the  interrupt,  bit-wise  logic 
operations  are  performed  on  the  register  with  its  default  setting  and  two  other  variables, 
RegisterMask  =  0x78  and  InterruptLevelS  =  0x03.  The  final  setting  on  the  register  is 
[01 1 1 101 1]  as  Figure  8.4.  TTie  values  in  the  bit  2:0  indicates  the  sonar  interrupt  level  being 
set  to  level  3. 
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Figure  8.4:  Setting  VMEbus  IRQ*  Interrupt  Control  Register 
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As  motion  system  control  interrupt  handler  installation,  we  can  install  the 
sonar  system  control  interrupt  service  routine  address  into  the  corresponding  interrupt 
service  routine  vector  location,  using  the  mk_handler()  library  function  as  below: 

ink_handler(S  onarIntV ector ,  *  S  onarSysControl) ; 

SonarIntVector:  Location  in  interrupt  handler  vector  table. 

*SonarSysContro:  Function  pointer  to  be  placed  into  handler  vector  table; 

C,  DYNAMIC  MEMORY  MANAGER  DESIGN 

Time  Complexity  and  space  complexity  have  been  two  important  measurement  of 
an  algorithm  in  evaluation  of  efficiency.  That  is  why  the  dynamic  memory  management  is 
so  important  in  a  software  development.  MML- 1 1  is  a  set  of  robot  control  functions  written 
in  ANSI  C.  The  C  language  has  already  had  memory  allocation/deallocation  functions  to 
manage  dynamic  memory  request.  Then  why  bother  to  install  them  in  MML-11?  The 
answer  is  because  Yamabico-1 1  is  a  self-contained  robot.  There  is  no  operating  system  in 
its  CPU  —  IV- SPARC  33  to  handle  the  memory  management.  In  order  to  use  the  available 
memory  in  SPARC  33  economically  and  efficiently,  MML-1 1  has  to  have  its  own  memory 
management  capability. 

The  most  common  known  methods  used  in  dynamic  memory  management  are  the 
following  buddy  systems  [28]: 

•  Binary  buddy  system. 

•  Fibonacci  buddy  system. 

•  Boundary  tag  buddy  system. 

We  adopt  Fibonacci  buddy  system  for  use  in  MML-11  for  the  following  two 
reasons.  Firstly  it  allows  for  a  greater  variety  of  possible  block  sizes  in  a  given  amount  of 
memory  than  Binary  buddy  system.  Second,  the  Fibonacci  buddy  system  provides  an 
efficient  method  to  allocate  and  deallocate  memory  blocks.  The  techniques  the  Fibonacci 
buddy  system  used  to  allocate  and  deallocate  memory  are  briefly  described  in  Appendix  B. 
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In  order  to  properly  design  a  memory  management  system,  the  memory  organization  of 
working  CPU  board  needs  to  be  understood. 

1.  IV-SPARC  33  Memory  Organization  Feature 

Yamabico-11  onboard  CPU  has  DRAM  of  size  16  Mega  bytes.  Its  memory 
organization  is  illustrated  in  Figure  6.1.  The  DRAM  array  is  organized  into  two- word 
(64-bit)  interleaved  banks  of  8  Mbytes  each.  Bidirectional  latching  data  buffers  direct  to 
and  from  the  two  banks.This  is  the  physical  memory  space  where  we  can  load  the  robot 
software  for  execution.  Loading  target  process  to  the  memory  is  in  upward  direction 
starting  from  the  bottom.  Although  DRAM  address  ranges  from  0x00000000  to 
OxOOFFFFFF,  the  space  between  0x00000000  and  0x00018000  is  reserved  for  system  use. 
Thus,  when  a  target  process  (the  compiled  programs  called  kernel)  is  downloaded  to  the 
SPARC  board,  it  starts  at  address  0x00001800.  Following  the  kernel,  the  uninitialized 
variables  are  placed  followed  by  the  initialized  variables.  The  top  address  of  the  memory 
block  is  occupied  by  the  target  process  and  its  variables  depends  on  the  size  of  the 
programs.  On  the  upper  part,  the  space  between  OxOOFFFFFF  and  OxOOFFFFOO  is  reserved 
for  future  use.  The  rest  of  memory  space  in  DRAM  will  be  used  for  stack  of  the  client’s 
program.  The  memory  used  by  the  stack  is  arranged  in  downward  direction  from  the  top 
the  DRAM  (actually  it  starts  at  the  address  OxOOFFFFOO). 

2.  Memory  Block  for  Dynamic  Allocation 

When  allocating  a  memory  block  to  a  request  dynamically,  caution  must  be  taken 
that  the  memory  space  designated  to  system  operation  should  not  be  interfered  with.  For 
SPARC-33  CPU,  it’s  obvious  that  the  only  space  that  can  be  used  for  this  purpose  is  the 
space  from  the  bottom  of  program  stack  down  to  the  top  of  memory  occupied  by  initialized 
variables.  Unfortunately,  those  addresses  are  not  fixed.  In  other  words,  it  is  difficult  to 
exactly  measure  what  space  will  be  available  for  dynamic  allocation  use.  However,  the  way 
CPU  handle  the  DRAM  helps  us  find  the  appropriate  space.  As  aforementioned,  the  target 
process  stack  will  consume  the  space  from  the  address  OxOOFFFFOO  downward.  In  the 
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meantime,  target  process  text  (program)  and  data  are  stored  upward  starting  from 
0x00018000.  In  order  to  prevent  possible  overlaps  between  the  data  in  dynamic  memory 
blocks  and  that  of  other  spaces,  oirr  policy  is  to  set  the  dynamic  memory  block  of  size  4 
Mbytes  in  the  middle  of  DRAM.  This  decision  of  choosing  proper  space  for  use  is 
reasonable.  By  looking  at  the  size  of  object  codes  of  current  MML-11  program,  we  found 
that  it  is  relatively  small  (about  400  kbytes)  compared  to  the  total  size  of  DRAM  in  SPARC. 
Therefore  we  decide  to  make  the  top  address  of  dynamic  memory  space  at  0x00800000 
which  is  exactly  the  midway  point  of  16  Mbytes  DRAM.  This  allows  for  a  stack  having 
about  8  Mbytes  to  consume  and  the  MML-11  kernel  can  be  extended  up  to  more  than  3 
Mbytes. 

3.  Memory  Block  Partition 

In  order  to  allow  a  memory  block  being  returned  to  the  available  list  to  be  coalesced 
with  any  other  block(s)  in  the  list,  the  memory  block  must  be  partitioned  in  such  a  way  that 
any  size  can  be  represented  as  the  sum  of  two  smaller  sizes  and  that  the  neighbor  blocks  are 
easy  to  identified.  The  most  famous  sequence  of  numbers  having  this  property  is  the 
Fibonacci  sequence.Therefore,  the  Fibonacci  buddy  system  is  adopted  for  memory  block 
partition  in  MML-11  operating  system  design  (see  Appendix  B).  We  design  dynamic 
memory  allocation  function  with  memory  block  partitioned  into  the  sizes  of  Fibonacci 
sequence  (in  byte)  as  follows: 

Fi  =  8 

F2=16 

Fi  =  F(i-i)  +  F(i-2)  fori  >2 

The  sequence  generated  is  as  follows:  <8,  16,  24,  40,  64,  104,  168,  272,  440,  712, 
1152,  1864,  3016,  4880,  7896,  12776,  20672,  33448,  54120,  87568,  141688,  229256, 
370944, 600200,  971144, 1571344,  2542488, 4113832>. 

Why  do  we  set  F^  =  8  instead  of  any  number  smaller?  This  is  because  IV-SPARC- 

33  memory  has  8  bytes  addressable  boundaries  and  this  size  of  the  block  will  be  used  to 
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compute  the  address  of  its  buddy  blocks.  Thus  all  partitioned  blocks  must  be  of  size  of 
multiple  of  8  to  meet  address  alignment  requirement.  This  important  feature  was  identified 
after  many  testing. 

4.  Dynamic  Memory  Allocation  Functions  Design 

As  stated  in  Appendix  B,  the  bookkeeping  data  is  needed  in  each  block  for 
Fibonacci  buddy  system  operation.  In  MML-1 1,  we  define  a  data  type  in  C  as  follows: 
typedef  struct  head{ 


unsigned  int 

Active:!; 

unsigned  int 

Lbc:7; 

unsigned  int 

Size; 

struct  head 

*next; 

struct  head 

*prev; 

}  HEADER; 

where  Active  is  used  to  indicate  whether  the  block  is  active  or  not.  The  unsigned  integer 
Lbc  is  used  to  maintain  a  record  of  how  deeply  the  block  is  nested  as  left  buddy  of  other 
blocks.  The  size  is  obviously  used  to  indicate  the  size  of  a  memory  block.  The  next  and  prev 
fields  of  the  structure  are  pointers  pointing  to  its  next  and  previous  block  in  the  available 
list  respectively.  When  a  block  is  active,  i.e.  being  assigned  to  a  request,  these  two  fields 
are  not  used.  The  algorithm  for  maintaining  this  left  buddy  count  involves  these  steps. 

•  As  a  block  is  split,  the  resulting  left  buddy  has  its  left  buddy  count  field 
increased  by  one.  The  resulting  right  buddy  has  its  left  buddy  count  field  set  to 
zero. 

•  As  coalescing  occurs,  the  left  buddy  must  always  have  its  left  buddy  count  field 
decreased  by  one. 

With  the  data  structure  for  bookkeeping  data  of  the  memory  block,  the  memory 
allocation  and  deallocation  functions  can  be  designed  using  the  general  algorithms 
described  in  [28]. 
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IX.  MML-11  LANGUAGE  SPECIFICATION 

A.  OVERVIEW 

In  this  chapter,  we  describe  the  design  of  user  functions  which  will  be  used  as 
interface  between  user  and  MML-11  software.  The  specifications  of  functions  for  motion 
control,  sonar  control  and  geometric  calculation  are  presented.  Some  of  basic  data 
structures  which  will  be  used  to  describe  the  functions  are  presented  also.  The  user 
functions  are  categorized  into  following  subsets: 

•  Geometric  functions 

•  Motion  planning  functions 

•  Motion  control  functions  including  sequential  functions  and  immediate 
functions. 

•  Sonar  control  functions. 

The  geometric  functions  defines  some  utility  functions  for  the  algebraic 
manipulation  of  geometric  variables.  The  motion  planning  functions  provide  the  user 
simple  interface  functions  to  build  a  world  model  and  to  conduct  motion  planning  when 
giving  a  specific  mission.  The  motion  control  functions  include  sequential  functions  and 
immediate  functions.  The  sequential  functions  define  a  set  of  motion  control  commands 
that  will  be  stored  in  a  buffer  when  they  are  used  in  the  user  program  and  will  be  executed 
sequentially  as  robot’s  background  tasks.  The  immediate  functions  define  the  commands 
which  take  effect  immediately  when  they  are  executed  in  user’s  program.  The  sonar  control 
functions  are  the  functions  used  to  control  sonar  operation  and  to  obtain  sonar  data. 

B.  DATA  STRUCTURE 

1.  Point 

The  POINT  structure  is  used  to  describe  a  position  in  two-dimensional  cartesian 
coordinates  system.  The  structure  includes  a  double  X  and  a  double  Y. 
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2.  Velocity 

The  VELOCITY  structure  is  used  to  describe  a  two  dimensional  velocity  vector. 
The  data  structure  is  made  up  of  two  doubles  that  represent  the  linear  and  rotational 
elements  of  velocity.  They  are  appropriately  named  Linear  and  Rotational,  respectively,  in 
the  VELOCITY  structure. 

3.  Configuration 

The  CONFIGURATION  is  the  standard  structure  for  describing  location  and 
direction  for  an  object.  It  consist  of  Posit,  with  type  of  POINT,  which  identifies  an  objects 
position  in  two  dimensional  cartesian  coordinates.  Another  element  is  Theta  of  type  double 
that  describe’s  the  object’s  orientation  in  relation  to  the  X  coordinate.  Finally,  there  is 
another  double  called  Kappa  that  represents  the  curvature  of  an  object’s  path. 

4.  Path  Element 

The  PATH_ELEMENT  data  structure  is  used  to  describe  and  store  the  various 
types  of  movements.  This  data  structure  consist  of  config  which  is  of  type 
CONFIGURATION.  It  holds  the  configuration  of  the  path  that  the  robot  is  to  follow. 
PATH_ELEMENT  also  contains  pathType,  which  is  of  type  PATH_TYPE.  A 
PATH_TYPE  is  a  data  structure  used  to  identify  the  various  paths  that  are  available  to  the 
robot.  It  consist  of  the  mode  which  is  of  type  MODE  and  class  which  is  of  type  CLASS. 
Type  MODE  is  an  enumeration  type  that  gives  a  name  to  each  path  that  the  robot  follows. 
Presently,  the  modes  that  are  available  include  NOMODE,  ENDMODE,  STOPMODE, 
PATHMODE,  ROTATEMODE,  BIDIRMODE,  KSPIRALMODE,  PARAMODE, 
FOLLOWMODE  and  REGIONMODE.  T5^e  CLASS,  which  is  also  an  enumeration  type, 
is  used  to  name  and  categorize  the  various  PATHMODE  types.  The  list  of  classes  include 
NOCLASS,  LINECLASS,  CIRCLECLASS,  BLINECLASS,  and  BDDIRCLASS. 
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C.  USER  FUNCTION  SPECIFICATION 
1.  Geometric  Functions 

a.  Euclidean  Distance 
Syntax:  double  euD\s(pl ,  p2) 

Parameters:  POINT  pi; 

POINT p2; 

Description: 

This  function  computes  an  returns  the  Euclidean  distance  between  two  given  points 

b.  Normalize 
Syntax:  double  novxniangle) 

Parameters:  double  angle; 

Description: 

This  function,  when  given  an  angle  in  radian,  returns  a  normalized  angle  between  - 
%  and  % .  This  is  the  most  common  normalizing  function  used  in  the  system. 

c.  Define  Configuration 

Syntax:  CONFIGURATION  defineConfig(x,  y,  theta,  kappa) 

Parameters:  double  x; 

double  y; 
double  theta; 
double  kappa; 

Description: 

When  passed  the  values  that  define  a  configuration  (x,  y,  theta,  kappa),  this  function 
allocates  and  assigns  a  configuration.  It  returns  a  configuration. 

The  configuration  can  be  used  to  represent  a  path  which  is  either  a  line  or  a  circle. 
If  the  configuration  is  defined  with  curvature  zero,  i.e.  kappa  =  0.0,  it  specifies  a 
straight  line  passing  through  the  point  {x,  y)  with  orientation  theta.  If  its  curvature 
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is  greater  than  zero,  i.e.  kappa  >  0.0,  the  path  is  a  counterclockwise  circle.  If  kappa 
<  0.0,  then  the  path  is  a  clockwise  circle.  Figure  9. 1  illustrates  these  concepts 


kappa  >  0.0  (counterclockwise) 


kappa  =  0.0  (straight  line) 


kappa  <  0.0  (clockwise) 


Figure  9.1:  A  Configuration  Represents  a  Line  or  a  Circle 


d.  Reverse  Configuration 

Syntax:  CONFIGURATION  reverseConfig(or/gma/) 

Parameters;  CONFIGURATION  original; 

Description: 

The  purpose  of  this  function  is  to  reverse  the  orientation  of  a  given  configuration  by 
180  degrees.  It  returns  the  reversed  configuration. 

e.  Inverse 

Syntax:  CONFIGURATION  inverse(ongma/) 

Parameters:  CONFIGURATION  original; 

Description; 

The  purpose  of  this  function  is  to  calculate  the  inverse  of  a  given  configuration  such 
that:  original  *  inverse  =  Identity.  The  parameter  —original  —  is  the  original 
configuration  in  global  coordinates.  This  function  returns  the  inverse  configuration. 
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f.  Compose 

Syntax:  CONFIGURATION  conipose(/zm,  second) 

Parameters;  CONFIGURATION//r^t; 

CONFIGURATION  second. 

Description: 

The  piupose  of  this  function  is  to  calculate  the  composition  of  two  configurations. 
Specifically,  the  function  takes  parameter  —first  and  composes  it  with  parameter  - 
-  second  to  calculate  and  return  the  composed  value.  The  retinned  value  is  the  goal 
configuration  in  global  coordinates. 

g.  Circular  Arc 

Syntax;  CONFIGURATION  Circular Arc(/,  alpha) 

Parameters:  double  /; 

double  alpha; 

Description: 

Given  a  tangential  orientation  alpha  and  the  arc  length  /  in  a  curve,  this  function 
computes  its  configuration  in  the  local  coordinate  system  [19].  In  motion  control 
case,  length  would  actually  be  delta- s  and  alpha  would  be  delta-theta.  The  function 
can  be  called  to  determine  the  configuration  after  the  incremental  move  in  the  local 
coordinate  system  of  the  original  configuration. 

h.  Intersection  of  Two  Lines 

Syntax:  CONFIGURATION  intersectLineToLine(^i,  q2) 

Parameters:  CONFIGURATION  ql ; 

CONFIGURATION  q2; 

Description; 

Given  two  configurations  representing  straight  two  lines,  this  function  calculates 
the  intersection  of  hnes  and  returns  the  configuration  of  the  intersection. 
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L  Depth 


Syntax:  double  depth(p,  alpha) 

Parameters:  POINT  p; 

double  alpha; 

Description: 

Given  a  point  p  and  the  orientation  of  the  point  alpha,  this  function  computes  the 
depth  of  the  point  along  a  line  defined  by  the  parameters.  It  returns  a  value  of  type 
double. 

2.  Motion  Planning  Functions 

a.  Creating  World  Model 
Syntax:  void  createModel(ivor/d) 

Parameters:  worldModel  world; 

Description: 

This  function  creates  a  world  model  decomposing  the  free  space  of  a  given  world 
into  K-regions.  It  will  generate  the  a  set  of  data  which  is  needed  in  planning  robot’s 
motion.  The  resultant  data  includes  a  world  model,  World,  a  region  table, 
RegionTable,  a  border  table,  BorderTable,  an  edge  table,  EdgeTable  and 
connectivity  graph,  CGraph. 

b.  Global  Path  Planning 

Syntax:  void  FindPath(wor/d,  startConfig,  goalConfig) 

Parameters:  worldModel  world, 

CONFIGURATION  startConfig, 

CONHGURATION  goalConfig; 

Description: 

This  function  finds  the  shortest  path  connecting  the  initial  region  and  the  final 
region  which  contains  start  configuration  and  goal  configuration  respectively.  The 
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output  of  this  function  is  a  sequence  which  begins  with  a  region  followed  by  a 
border  repeatedly  and  ends  with  a  region.  In  the  sequence,  the  first  element  is  the 
region  which  contains  start  configuration  and  the  last  element  is  the  region  where 
the  final  configuration  resides.  The  border  between  two  regions  in  the  sequence  is 
their  common. 

c.  Local  Motion  Planning 

Syntax;  void  LocalMP(wor/d,  startConfig,  goalConfig,  path) 

Parameters:  worldModel  world, 

CONFIGURATION  startConfig, 

CONHGURATION  goalConfig; 
pathClass  path; 

Description: 

This  function  generates  the  motion  instructions  for  each  regions  along  the  path. 
Those  instructions  will  be  taken  to  drive  the  robot  in  each  region  and  finally  stop 
the  robot  at  the  final  configuration. 

3.  Motion  Control  Sequential  Functions 

The  sequential  functions  define  a  set  of  motion  control  commands  which  are  stored 
in  a  buffer  that  acts  as  an  interface  between  user  and  robot.  When  the  user  program  is  being 
executed,  commands  of  this  type  included  in  the  user  program  do  not  take  effect 
immediately  instead  they  are  loaded  in  buffer  as  motion  instructions.  The  motion  control 
system  reads  the  instructions  from  the  top  of  buffer  sequentially  and  controls  the  robot’s 
motion  accordingly.  The  transition  control  from  one  instruction  to  another  is  described  in 
Chapter  DC.  The  specifications  of  those  functions  are  hsted  below. 

a.  Tracking  a  Line 
Syntax:  void  line(^) 

Parameters:  CONFIGURATION  q-, 
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Description: 

The  function  defines  a  command  that  orders  the  robot  to  follow  the  line  or  circle 
specified  by  the  configuration  q.  If  the  robot’s  last  configuration  before  the 
command  is  executed  is  not  on  the  track  of  the  line  specified,  the  robot  uses  steering 
function  to  transfer  to  the  line  with  smooth  motion.  Figure  9.2  illustrates  robot’s 
behavior  when  executing  line(^)  with  a  straight  line  q. 


Figure  9.2:  The  Line  Function 


b.  Tracking  the  Line  from  Its  Back  and  Stopping 
Syntax:  void  stopO(^) 

Parameters:  CONFIGURATION  q; 

Description: 

This  function  defines  a  command  which  steers  the  robot  to  track  the  line  specified 
by  the  configuration  q  from  its  back.  If  the  robot’s  image  is  on  the  back  half  of  the 
line,  the  robot  tracks  the  line  as  function  line()  and  stops  when  its  image  reaches  the 
configuration.  If  the  robot’s  image  falls  on  the  forward  part  of  the  line  initially,  the 
robot  would  not  move,  (see  Figure  9.3) 
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The  robot  would  not 
move  in  this  case 


Figure  9.3:  The  Backward  Line  TVacking  with  Stop 

c.  Symmetrically  Tracking  a  Line  from  Its  Back  with  Stopping 
Syntax:  void  stopl(^) 

Parameters:  CONFIGURATION  q-. 

Description: 

This  function  defines  a  command  which  steers  the  robot  to  track  the  line  specified 
by  the  configuration  q  from  its  back  with  a  symmetric  trajectory.  This  type  of  line 
tracking  is  called  symmetric  line  tracking.  Let  q^  be  the  robot’s  initial  configuration 
and  q^^  be  a  line  specified  by  the  reversed  configuration  of  q^.  Assuming  q^  is  the 
reversed  configuration  of  the  parameter  q  passed  in  by  the  function.  In  the 
symmetric  line  tracking,  a  forerunner  (virtual  robot)  simulation  is  needed  to 
generate  a  trajectory  mnning  back  from  q^  by  tracking  the  reference  line  specified 
by  q^^.  The  following  steps  are  taken  in  order: 

•  Stepl:  While  the  real  robot  is  moving  straight  forward  (by  tracking  the  line 
specified  by  the  initial  configuration  qf),  run  the  forerunner  backward  as  stated 
above  and  store  the  trajectory  in  a  path  n  by  appending  a  reverse  configuration 
(negate  its  curvature  also)  of  forerunner’s  current  configuration. 

♦  Step  2:  Compare  real  robot’s  position  against  forerunner’s.  If  they  do  not 
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meet,  continue  Stepl.  Otherwise,  stop  the  forerunner  simulation  and  start  to 
track  the  prestored  path  11  until  the  robot  reach  the  configuration  q. 

•  Step  3.  When  the  configuration  q  is  reached,  the  robot  stops. 

Figure  9.4  illustrates  the  symmetric  line  tracking  concept.  In  using  this  function,  the 
caution  must  be  taken  that  it  is  user’s  responsibility  to  make  sure  there  is  enough 
distance  between  initial  configuration  q^  and  configuration  q  to  allow  forerunner’s 
trajectory  to  converge  to  its  reference  line.  Otherwise  the  real  robot  and  forerunner 
may  not  meet  each  other  and  it  may  cause  an  unpredictable  robot  behavior. 


Forerunner 


Figure  9.4:  The  symmetric  Line  Tracking  with  Stop 


d.  Symmetrically  Tracking  a  Line  from  Its  Back  without  Stopping 
Syntax:  void  pass(^) 

Parameters:  CONFIGURATION  q; 

Description: 

This  function  defines  a  command  which  can  make  a  robot  have  similar  behavior  as 
symmetric  line  tracking  defined  in  function  stopl()  except  it  will  not  stop  at  the 
configuration  q.  Instead,  when  robot  pass  configuration  q,  it  keeps  tracking  the  line 
specified  by  q. 

e.  Rotation 

Syntax:  void  Rotate{theta) 

Parameters:  double  theta; 
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Description: 

This  function  is  to  command  the  robot  to  rotate  a  given  angle.  Negative  angle  makes 
the  robot  rotate  clockwise,  while  positive  angle  makes  an  counterclockwise 
rotation. 

/.  5 witch  Robot’s  Heading  Direction 
Syntax:  void  switchDir() 

Parameters:  void; 

Description: 

This  function  reverses  the  heading  direction  of  the  robot. 

g.  Set  Robot’s  Configuration 
Syntax:  void  setRobotConfig((3') 

Parameters:  CONFIGURATION 
Description: 

This  function  sets  robot's  configuration  to  a  given  configuration  q. 

4.  Motion  Control  Immediate  Functions 

a.  Set  Path  Element 

Syntax:  void  setPathE\emmt{newPath) 

Parameters:  PATH_ELEMENT  newPath; 

Description: 

This  function  sets  the  value  of  the  current  path  element  in  motion  control  to  the 
path  element  passed  in  as  a  parameter. 

b.  Get  Path  Element 

Syntax:  PATH_ELEMENT  getPathElement(vo/d) 

Parameters:  void; 

Description: 

This  function  retrieves  the  current  path  element  in  the  motion  control  module. 


c.  Set  Robot’s  Configuration  Immediately 
Syntax:  void  setRobotConfiglmni(g) 

Parameters:  CONFIGURATION  q; 

Description: 

This  function  sets  robot's  configuration  to  a  given  configuration  q  immediately. 

d.  Set  Robot’s  Linear  Speed  Immediately 
Syntax:  void  setLmVellmm(speed) 

Parameters:  double  speed. 

Description: 

This  function  sets  the  robot's  linear  velocity  immediately. 

e.  Set  Robot’s  Rotational  Speed  Immediately 
Syntax:  void  setRotVelImm(5pee(i) 

Parameters:  double  speed. 

Description: 

This  function  sets  the  robot's  rotational  velocity  immediately. 

/.  Set  Robot’s  Linear  Acceleration  Immediately 
Syntax:  void  setLinAccImm(acc) 

Parameters:  double  acc\ 

Description: 

This  function  sets  the  robot's  linear  acceleration  immediately. 

g.  Set  Robot’s  Rotational  Acceleration  Immediately 
Syntax:  void  setRotAccImm(racc) 

Parameters:  double  race; 

Description: 

This  function  sets  robot’s  angular  acceleration  for  speed  changes  in  rotation. 
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h.  Sets igma Immediately 
Syntax:  void  setSigniaImm(s/^ma) 

Parameters:  double  sigma; 

Description: 

This  function  sets  the  robot’s  Sigma  which  control  the  sharpness  of  it  trajectory 
when  the  robot  is  turning. 

i.  Set  T otal  Distance  traveled  Immediately 
Syntax:  void  setTotalDistancelinm(rfwrance) 

Parameters:  double  distance; 

Description: 

sets  the  total  distance  travelled  by  the  robot  to  the  value  passed  as  a  parameter 

j.  Get  Total  Distance  traveled  Immediately 
Syntax:  double  getTotalDistanceImin(voicO 
Parameters:  void; 

Description: 

Returns  the  total  distance  travelled  by  the  robot. 

k.  Stop  Immediately 
Syntax:  void  stoplmin(void) 

Parameters:  void 
Description: 

This  function  stops  the  robot  immediately  with  the  current  acceleration  rate  until  the 
speed  reaches  0. 

/.  Halt  the  Robot  Immediately 

Syntax:  void  haltlinm(vc>/(i) 

Parameters:  void 
Description: 
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The  function  brings  the  robot  to  a  stop  with  the  current  acceleration  rate.  The  robot 
stays  in  the  state  until  resumelmm()  function  is  called.  When  the  resumelmm() 
function  is  called,  it  resumes  the  original  motion  without  changes. 

m.  Resume  the  Robofs  Motion  Immediately 
Syntax:  void  resuineImm(voj(i) 

Parameters:  void 
Description: 

The  function  allows  the  robot  to  resume  the  motion  it  was  executing  before  the 
haltImmO  command  was  given. 

n.  Set  the  Robots  Motor  On 
Syntax:  void  MotionOn(voz(i) 

Parameters:  void 

Description: 

Enables  the  motor  control  functionality. 

0.  Set  the  Robofs  Motor  Off 
Syntax:  void  MotionOn(vo/<i) 

Parameters:  void 
Description: 

Disables  the  motor  control  functionality.  By  calUng  this  function,  users  can  push 
Yamabico  as  they  like. 

p.  Logging  Motion  Data 

Syntax:  void  Motionlog(*F/tenam^,  Frequency,  BufferSize) 

Parameters:  char  ^Filename,  int  Frequency,  int  BufferSize; 

Description: 

This  function  prepares  the  tracing  system  to  log  motion  data.  Tracing  is 
automatically  turned  on  after  this  function  is  called.  The  Filename  specifies  a  file 
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name  that  will  be  used  to  store  data  when  the  data  is  uploaded  to  the  host.  The 
Frequency  specifies  how  many  sonar  cycles  are  skipped  before  data  is  logged. 

5.  Sonar  Control  Functions 

a.  Enable  Sonar 

Syntax:  void  F<nab\eSonsa'{sonarNumber) 

Parameters:  int  sonarNumber 
Description: 

This  function  enables  the  sonar  group  that  contains  sonarNumber,  which  causes  all 
the  sonars  in  that  group  to  echo-range  and  write  data  to  the  data  registers  on  the 
sonar  control  board. 

b.  Disable  Sonar 

Syntax:  void  DisableSonar(.sonarNMAw&cr) 

Parameters:  int  sonarNumber 
Description: 

This  function  removes  the  sonar  sonarNumber  from  the  enabled_sonars  Mst.  If 
sonar  sonarNumber  is  the  only  enabled  sonar  from  it’s  group,  then  the  group  is 
disabled.  Otherwise,  the  group  will  continue  echo  ranging  until  all  sonars  in  group 
are  disabled. 

c.  Logging  Sonar  Data 

Syntax:  void  SQin2urVog{Frequency,  BufferSize,  SonarNumber,  LogType) 
Parameters:  mi  Frequency,  int  BufferSize,  int  SonarNumber,  int  LogType; 
Description: 

This  function  prepares  the  tracing  system  to  log  sonar  data.  The  Frequency  specifies 
how  many  sonar  cycles  are  skipped  before  data  is  logged.  The  Buffersize  specifies 
how  many  bytes  of  storage  to  allocate  to  save  the  data.  The  SonarNumber  indicates 
the  sonar  you  wish  to  log  data.  The  LogType  specifies  the  type  of  data  to  be  logged. 
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The  type  of  logging  data  could  be  SONAR_RAW  which  logs  the  sonar  range, 
SONAR_GLOBAL  which  logs  the  object’s  x,  y  coordinates  and  its  range, 
SONAR_SEGMENT  which  logs  segment  data,  and  SONAR_ALL  which  indicates 
to  log  all  three  types  of  data. 

d.  Get  Sonar  Returns 

Syntax:  double  SonsarisonarNumber) 

Parameters:  int  sonarNumber 
Description: 

This  function  returns  the  distance  (in  centimeters)  sensed  by  the  sonarNumber-th 
ultrasonic  sensor.  If  no  echo  is  received,  an  INFINITY  (999999.0)  is  returned.  If  the 
distance  is  less  than  10  cm,  then  a  0  is  returned. 

e.  Get  Global  Sonar  Returns 
Syntax:  POINT  Global(sonarNumber) 

Parameters:  int  sonarNumber 
Description: 

This  function  returns  the  data  of  type  POINT  which  indicates  the  global  x  and  y 
coordinates  of  the  position  of  the  last  sonar  return. 

/.  Get  Segment 

Syntax:  Segment  GeiSegment{sonarNumber) 

Parameters:  int  sonarNumber 
Description: 

This  function  returns  the  pointer  to  the  oldest  completed  unread  segment  of  the 
sonar  passed  in.  If  there  is  no  completed  unread  segment  NULL  is  returned. 

g.  Enable  Linear  Fitting 
Syntax:  void  EnableLinearFitting(j'onarA/'Mm&cr) 

Parameters:  int  sonarNumber 
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Description: 

This  function  enables  the  linear  fitting  algorithm  to  be  active.  The  algorithm  gather 
data  points  from  sonar  sonarNumber  and  form  them  into  line  segments. 

h.  Disable  Linear  Fitting 
Syntax:  void  DisabIeLinearFitting(^onflrNMm&er) 

Parameters:  int  sonarNumber 
Description: 

This  function  causes  sonar  system  to  cease  forming  line  segments  for  sonar 
sonarNumber. 
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X.  SENSOR-BASED  MOTION  NAVIGATION 


A.  SENSOR-BASED  MOTION  CONTROL 

The  most  typical  way  of  using  steering  function  is  tracking  a  path  specified  by  a 
configuration  q.  When  the  robot  is  tracking  the  path,  Afc,  A0,  and  Ad  can  be  computed  by 
motion  control  subsystem  to  determine  the  motor  control  value  and  eventually  be  used  to 
drive  the  wheels  in  a  short  interval  (which  is  10  ms  in  MML-1 1). 

In  addition  to  path  tracking,  there  are  some  other  flexible  motion  control  methods 
which  are  helpful  in  local  motion  planning.  This  method  requires  integration  of  robot’s 
sensors  with  motion  control  algorithm  while  not  using  path  segment.  Since  motion  control 
is  basically  based  on  the  sensor  returned  information,  we  called  it  a  sensor-based  motion 
control. 

The  essential  idea  of  this  new  method  is  based  on  the  fact  that  obstacles  present  in 
the  working  environment  and  the  sensors  are  able  to  detect  those  obstacles  and  to  return 
their  distances  for  processing.  We  assume  that  the  obstacles  are  all  of  rectilinear  polygons 
and  they  have  walls  and  corner  on  their  outer  appearance.  Therefore,  it  is  possible  for  a 
robot  to  travel  in  the  free  space  along  obstacles’  outer  boundary  and  to  keep  certain  constant 
safety  clearance  (Safety  clearance  concept  is  to  be  defined  in  Section  B).  We  name  this  kind 
of  robot’s  behavior  as  wall-following.  Since  keeping  clearance  from  objects  is  important 
in  wall-following  motion,  the  robot  will  travel  along  a  wall,  follow  a  wall  in  other  words, 
with  clearance  required  when  it  is  available  or  desired.  But  when  a  comer  is  eventually  met 
in  wall-following,  the  robot  needs  to  change  its  orientation  to  keep  following  the  object. 
During  the  robot  changing  its  heading  orientation,  it  is  traveling  along  the  comer,  following 
the  corner  in  other  words,  trying  to  keep  the  required  clearance  from  the  object  so  that  it 
can  continue  to  perform  the  same  motion  when  a  wall  is  available  again.  Since  the  walls 
and  comers  are  interpreted  as  edges  and  vertices  of  polygons  in  geometry,  we  name  the  two 
different  motions  described  above  as  edge-following  motion  and  vertex-following  motion 
respectively.  The  wall-following  motion  can  be  either  right-wall-following  or  left-wall- 


following  depending  upon  right  clearance  or  left  clearance  the  robot  is  trying  to  keep  from 
objects  for  navigation.  For  example,  if  the  robot  intends  to  keep  its  left  clearance  with 
objects,  the  wall  on  the  robot’s  left  will  be  used  for  navigation,  and  then  the  wall-following 
motion  is  called  left-waU-following  motion. 

B.  CLEARANCE  DEFINITION 

In  wall-following  Motion  the  robot  will  use  sensors  for  navigation,  thus  safety 
clearance  is  an  important  factor.  For  a  better  understanding  of  how  the  sensors  are  used  in 
motion  planning,  first  we  define  some  terminologies  related  to  safety  clearance.  The 
clearance  wq  is  defined  as  the  distance  from  the  robot’s  outside  edge  of  the  wheels  to  the 
object.  The  distance  data  the  sensor  returns  is  raw  data  which  indicates  how  far  the  object 
is  from  the  sensor.  With  the  sensor  configuration  in  hand,  the  clearance  Wq  can  be 
computed.  The  robot’s  safety  distance  Wj  is  defined  as  the  summation  of  clearance  Wq  and 
half  of  robot’s  width  HWidth.  Figure  10.1  illustrates  the  definitions  of  clearance  elements. 


Figure  10.1:  The  Definitions  of  Robot’s  Clearance  Elements 
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C.  EDGE-FOLLOWING  MOTION 

While  there  is  a  wall  on  either  side  of  the  robot  and  robot  is  trying  to  keep  itself 
away  from  the  wall  with  a  safety  distance  wi,  it  is  following  an  edge.  We  define  this  as  the 
robot  is  in  edge-following  mode.  We  apply  the  steering  function  in  Eq  3. 1  to  control  the 
robot  while  the  robot  is  in  edge-following  mode.  Let  the  robot’s  current  configuration  be  q 
=  {p,  0,  fc).  The  variables  Ale,  A0,  and  Arf  in  steering  function  can  be  computed  based  on 
some  assumptions  as  followings.  The  desired  curvature  of  the  wall  is  zero  because  we 
assume  the  wall  is  flat  like  a  line.  The  desired  orientation  of  the  wall,  0^,  can  be  computed 
before  the  robot  is  following  the  waU.  This  desired  orientation  is  orthogonal  because  we 
assume  the  environment  consists  of  rectilinear  polygons.  The  distance  between  the  robot 
and  the  wall,  cl^,can  be  obtained  by  using  sensors.  Therefore  the  variables  for  the  steering 

function  are  defined  as; 

Ale  =  k; 

A0  =  0  -  0^,’ 

Ad=wj-  d^; 

Figure  10.2  shows  the  robot  in  edge-following  mode  in  left-wall-following  type 


Figure  10.2:  The  Edge-following  Mode  in  Left-wall-following 
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D.  VERTEX-FOLLOWING  MOTION 


In  a  normal  polygon  (convex  polygon),  when  the  robot  is  coming  to  the  end  of  an 
edge,  the  sensors  can  detect  a  convex  corner  which  is  interpreted  as  a  convex  vertex.  In 
order  to  keep  safety  clearance  from  the  object,  the  robot  need  to  turn  around  the  vertex  with 
a  circle  motion  taking  the  vertex  as  its  center  (that  was  obtained  by  sensor)  and  safety 
clearance  w;  as  its  radius.  We  define  the  second  mode,  named  vertex-following  mode,  in 
the  sensor-based  motion  control  for  this  situation.  Let’s  take  left- wall-following  as  an 
example.  The  curvature  of  the  circle  then  is  calculated  as  7  /  r,  where  r  is  the  circle’s  radius. 
Let  the  robot’s  current  configuration  h&q  =  (p,  6,  k)  and  9^  be  the  desired  orientation  on 
the  circle.  The  distance  between  robot’s  current  position  and  center  of  the  circle,  d^,  can  be 
easily  computed.  Therefore,  the  steering  function  variables  are  computed  as  below: 

Ak  =  k-  Hr; 

A0  =  0  -  0^; 

Ad  =  r  -  d^; 

Figure  10.3  illustrates  the  vertex-following  in  left-wall-following. 


Figure  10.3:  The  Vertex-following  Mode  in  Left-wall-following 
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For  an  inverted  polygon  or  a  general  polygon,  the  corner  can  be  either  convex  or 
concave  one.  The  motion  of  following  a  convex  corner  are  described  as  above.  For  a 
concave  corner,  the  same  idea  of  vertex-following  stated  above  will  be  applied  to  the 
concave  vertex  situation  except  the  calculation  of  the  center  (the  vertex)  of  the  circle. 
Figure  10.4  illustrates  the  computation  of  the  center  of  circle  vertex-following  on  concave 
corner  in  left- wall-following. 


Figure  10.4:  The  Vertex-Following  on  Concave  Corner 


E.  WALL-FOLLOWING  MOTION  CONTROL 

This  section  describes  how  we  control  the  robot’s  motion  in  wall-following  motion 
planning.  We  define  four  states  for  controlling  robot  in  accordance  with  the  modes  defined 
in  wall-following  motion  as  follows: 


EDGE, 


•  EDGED, 

•  CONVEX,  and 

•  CONCAVE. 

When  the  robot  is  in  the  edge-following  mode,  the  state  is  defined  as  EDGE  state. 
The  robot  will  adjust  itself  to  follow  the  wall  with  given  safety  distance  Wj  by  computing 
steering  function  variables  as  described  previously.  When  the  robot  is  in  the  vertex¬ 
following  mode  and  the  corner  is  found  as  a  convex  one,  we  define  the  robot’s  motion  is  in 
CONVEX  state.  When  the  robot  is  in  the  vertex-following  mode  and  the  corner  is  a 
concave  one,  we  define  it’s  in  CONCAVE  state.  In  both  CONVEX  and  CONCAVE  states, 
the  robot  will  take  the  computed  center  as  a  vertex  and  turn  around  the  vertex  with  a  circle 
motion.  Because  of  the  limitation  of  sensors,  it  is  difficult  to  compute  the  exact  position  of 
circle’s  center  for  the  motion  to  follow  a  convex  comer.  Therefore,  an  intermediate  state 
EDGED  is  created  to  prevent  unexpected  distance  returned  from  sensors  being  used.  The 
intermediate  state  EDGED  is  a  transition  state  between  CONVEX  state  and  EDGE  state. 
The  transitioning  from  CONVEX  to  EDGED  is  controlled  by  checking  the  angle  the  robot 
turned  in  the  circle  motion.When  the  robot  has  turned  desired  angle  (normally  it  is  9D 
degree),  the  state  changes  from  CONVEX  to  EDGED.  While  the  robot  is  in  EDGED  state, 
it  follows  a  desired  path,  which  is  computed  at  the  beginning  of  CONVEX  state,  until  it 
passes  the  point  where  the  robot  can  start  the  edge-following.  Then  the  robot  changes  the 
state  from  EDGED  to  EDGE. 

At  the  beginning  of  Wall-Following  motion,  the  robot  is  assumed  in  EDGE  state. 
While  the  robot  is  following  the  edge,  the  state  can  be  changed  according  to  the 
environment.  If  the  comer  is  not  detected,  it  stays  in  EDGE  state.  If  a  convex  comer  is 
identified,  the  robot  switches  its  state  from  EDGE  to  CONVEX.  Similarly,  if  a  concave 
corner  is  found,  the  robot  switches  its  state  from  EDGE  to  CONCAVE.  While  the  robot  is 
in  CONVEX  or  CONCAVE  states,  the  sensors  are  not  used.  Before  the  turning  of  desired 
angle  is  finished,  the  robot  stays  in  its  original  state.  At  the  end  of  CONVEX  states,  the 
robot  switches  the  state  to  EDGED.  Then  it  switches  to  the  EDGE  state  according  to  the 
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position  checking.  Figure  10.5  illustrates  the  states  change  in  wall-following  in  a  convex 
corner.  The  state  change  from  CONCAVE  is  directly  to  EDGE  when  the  vertex-following 
is  finished.  Figure  10.6  illustrates  the  state  change  in  a  concave  comer.  Figure  10.7  shows 
the  transition  of  robot’s  states  in  wall-following  motion. 

i 

EDGE  state 

EDGEO  state 

CONVEX  state 

EDGE  state 

Figure  10.5:  The  State  Change  in  Wall-following  Motion  I 


EDGE  state 
CONCAVE  state 

EDGE  state 

Figure  10.6:  The  State  Change  in  Wall-following  Motion  n 
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Figure  10,7:  The  State  Transition  of  the  Wall-following  Motion 


F.  ALGORITHM 

The  algorithm  for  the  wall-following  motion  is  as  following: 
Algorithm  FollowRule  (va,  vc) 

Input:  va:  the  actual  velocity,  vc:  the  commanded  velocity. 
Output:  Commanded  linear  and  rotational  speeds 

(I)  get  the  desired  clearance  wO 
(2  )  case  EDGE  : 

(3)  get  sensor  distance  dist 

(4)  if  convex  corner  not  found  then 

(5)  if  concave  corner  not  found  then 

(6)  get  vertex  for  center  of  circle  motion 

(7)  get  desired  orthogonal  orientation 

(8)  compute  Ad  of  edge -following 

(9)  else 

(10)  compute  circle  radius 

(II)  compute  circle  curvature 

(12)  compute  center  of  circle 
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(13)  compute  desired  orthogonal  orientation 

(14)  compute  robot’s  desired  orientation 

(15)  compute  desired  curvature 

(16)  compute  Ad  of  vertex-following 

(17)  switch  state  to  CONCAVE 

(18)  caseEDGEO: 

(19)  if  not  finish  turning  then 

(20)  compute  Ad  of  the  desired  pathfollowing 

(21)  else 

(22 )  compute  Ad  of  edge-following 

(23)  switch  state  to  EDGE 

(24)  case  CONCAVE: 

(25)  if  not  finish  turning  then 

(26)  compute  robot’s  desired  orientation 

(27)  compute  Ad  of  vertex-following 

(28)  else 

(29)  compute  robot’s  desired  orientation 

(30)  compute  desired  curvature 

(31)  compute  Ad  of  edge-following 

(32)  switch  state  to  EDGE 

(33)  case  CONVEX: 

(34)  if  not  finish  turning  then 

(35)  compute  robot’s  desired  orientation 

(36)  compute  Ad  of  vertex-following 

(37)  else 

(38)  compute  robot’s  desired  orientation 

(39)  compute  desired  curvature 

(40)  compute  Ad  of  edge-following 

(41)  switch  state  to  EDGEO 

(42)  compute  Ak.  and  A0 

(43 )  compute  Ak  /  As  (usir^  steering  Junction) 

(44)  compute  commanded  linear  and  rotational  speeds 

(45)  return  commanded  linear  and  rotational  speeds 


G.  IMPLEMENTATION 

The  robot’s  motion  is  controlled  by  a  motion  control  system.  In  Yamabico-11  this 
module  is  invoked  every  10  milliseconds.  To  implement  wall-following  motion,  we  need 
to  create  new  motion  rules  other  than  the  regular  motion  rules  which  are  used  for  path 
tracking  motion  control.  These  rules  are  named  LeftFollowRule  and  RightFoIIowRule. 

1.  Simulation 

The  use  of  simulation  to  verify  the  theory  is  important  in  scientific  research.  As  we 
know  the  desired  curvature  of  edge-following  is  zero  since  we  are  assuming  the  wall  is  a 
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flat  wall  which  will  be  a  straight  line  if  it  is  projected  on  the  two  dimensional  plan.  But  the 
desired  curvature  of  vertex-following  will  be  the  curvature  of  desired  circle  which  the  robot 
is  supposed  to  follow.  In  order  to  maintain  a  smooth  motion,  the  robot’s  curvature  is  not 
allowed  to  change  abruptly.  We  were  not  sure  whether  the  robot’s  curvature  would  be 
changing  smoothly  or  not  when  the  following  mode  changed.  Instead  of  implementing  it 
on  real  robot  directly,  we  first  simulate  the  mode  change.  Figure  10.8  illustrates  the 
trajectory  of  wall-following  motion  simulation.  Because  we  can  not  simulate  sonar’s 
operation,  we  assume  the  sonar  always  returns  the  safety  distance.  In  other  words,  the  Ad 
is  always  zero.  Therefore,  the  simulated  trajectory  is  actually  a  reference  path  for  the  real 
time  robot’s  motion.  From  this  simulation  results,  we  know  that  the  robot’s  curvature  will 
change  smoothly  while  it  switches  from  one  following  mode  to  another. 
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Figure  10.8:  The  Trajectory  of  Wall-following  Simulation 
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2.  Experimental  Results  of  Real  Time  System 

The  implementation  of  wall-following  motion  will  naturally  require  the  utilization 
of  sensors.  In  Yamabico-1 1,  the  available  sensors  are  12  sonars  which  are  installed  around 
the  robot  with  30  degrees  apart  from  one  another  in  their  orientations.  Figure  6.3  illustrates 
the  configurations  of  the  sonars  in  Yamabico-1 1. 

To  follow  a  wall,  the  left  and  right  sonars,  #  5  and  #  7,  will  be  used  to  detect  the 
distance  of  the  wall  on  right  and  left  sides  respectively,  so  that  the  motion  rules  can  steer 
the  robot  to  foUow  a  wall  keeping  the  specified  safety  distance  from  the  wall.  The  sonar  # 
0  in  front  of  robot  is  mainly  used  to  check  the  wall  in  front. 

A  lot  of  experiments  have  been  done  using  real  time  robot  after  simulation 
succeeded.  Figure  10.9  illustrates  the  trajectory  of  the  real  time  robot  Yamabico-1 1 
traveling  around  a  box  with  right-wall-foUowing  motion.  This  proves  that  the  robot  is  able 
to  navigate  itself  by  using  its  senors  in  an  unknown  environment. 
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Figure  10.9:  The  Trajectory  of  Real  Time  Wall-following  Motion 
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XL  IMPLEMENTATION 


A.  OVERVIEW 

This  chapter  describes  how  to  implement  the  local  motion  planning.  The 
implementation  of  global  motion  planning  was  described  in  [7].  In  the  chapter,  the  data 
structures  for  end-portion  motion  planning  and  mid-portion  planning  are  addressed  first. 
Then  the  algorithms  of  implementation  are  discussed.  At  last,  the  experimental  results 
conducted  by  Yamabico-11  using  MML-1 1  software  system  will  be  presented. 

B.  DATA  STRUCTURES 

This  section  describes  the  data  structures  for  implement  local  motion  planning. 
Since  end-portion  motion  planning  is  more  comphcated,  it  requires  a  data  structure  which 
is  different  from  the  data  structure  used  in  mid-portion  motion  planning  to  hold  the  motion 
instructions.  We  will  describe  the  data  structure  for  implementing  end-portion  motion 
planning  first.  After  that,  the  data  stmcture  for  implementing  mid-portion  motion  planning 
will  be  discussed. 

1.  Data  Structure  for  End-portion  Motion  Planning 

In  the  subsection  we  discuss  the  main  data  stracture  to  hold  motion  instruction  of 
the  results  of  end-portion  motion  planning.  The  intermediate  data  structures  required  by 
planning  operations  are  straightforward  as  described  in  Chapter  V.  In  end-portion  motion 
planning,  both  forward  and  backing  up  motion  may  be  required  to  accomphsh  the  task. 
Thus,  the  data  stmcture  needs  to  reflect  this  requirement  in  motion  instruction.  In  order  to 
make  motion  control  (execution)  as  simple  as  possible,  for  any  single  motion,  the  data 
structure  required  includes: 

•  Reference  path:  The  reference  path  stores  the  whole  path  information  which 
will  be  followed.  A  path  is  normally  represented  by  configurations  which 
include  position,  orientation  and  curvature.  Since  the  reference  path  could  be  a 
straight  line  or  a  curve  generated  by  simulator,  it  may  consist  one  or  more  than 


one  configuration.  Thus,  a  dynamic  structure  is  needed  for  this  data  element  to 
store  finite  number  of  configurations.  A  linked  list  will  be  used  to  construct  the 
reference  path,  (see  Figure  11.1). 


NULL 


Figure  11.1:  Reference  Path  Data  Structure 


•  Sigma:  The  sigma  is  an  important  information  for  conducting  path  tracking.  It 
will  be  used  to  decide  the  smoothness  of  tracking  trajectory  while  this  motion 
instruction  is  taken.  The  type  of  this  element  is  double. 

•  Motion  type:  This  element  indicates  whether  the  motion  is  forward  motion  or 
backing  up  motion. 

•  End  configuration:  This  is  a  type  of  configuration  which  indicates  the  last 
configuration  the  robot  needs  to  transition  to  next  motion  instruction. 

•  Stop:  This  element  indicates  whether  the  motion  need  to  stop  or  not.  Normally, 
a  backing  up  motion  needs  to  stop  at  the  end  configuration  as  indicated.  A 
forward  motion  may  need  to  stop  in  the  case  of  a  backing  up  motion  following. 

•  Next:  This  is  a  pointer  pointing  to  next  motion  instruction. 

As  aforementioned,  end-portion  motion  could  be  a  combination  of  many  forward 
and  backing  up  motions.  However,  the  number  of  different  motions  is  dynamic.  Therefore, 
the  data  structure  should  be  designed  to  satisfy  this  need.  Figure  11.2  illustrates  the  entire 
data  structure  for  end-portion  motion  planning. 
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Figure  11.2:  End-portion  Motion  Data  Structure 


2.  Data  Structure  for  Mid-portion  Motion  Planning 

Although  there  are  many  different  kind  of  combined  motions  in  the  results  of  mid¬ 
portion  motion  planning,  we  can  design  a  simple  data  structure  to  contain  all  kind  of  motion 
instructions.  The  basic  information  needed  for  the  motion  in  a  region  includes: 

•  Region  ID, 

•  Entrance  configuration, 

•  Reference  path, 

•  Sigma, 

•  Initial  tracking  configuration, 

•  Exit  border  ID  and  its  corresponding, 

•  Exit  configuration. 

For  instance,  the  simplest  motion  can  be  planned  to  track  a  reference  line  of  exit 
configuration  of  a  region.  The  example  can  be  found  in  Figure  4. 13.  To  specify  this  motion, 
we  can  store  the  exit  configuration  in  Reference  path  and  indicate  where  to  start  the 
tracking  in  Initial  tracking  configuration  and  store  the  proper  smoothness  in  Sigma.  Then 
the  robot  can  conduct  the  motion  as  shown  in  the  figure.  Is  it  possible  to  use  such  a  simple 


185 


data  structure  to  specify  a  more  complicated  motion  as  the  two  perpendicular  line  tracking 
in  Figure  4.16  (b)?  The  answer  is  yes.  As  long  as  the  information  of  reverse  path  and  the 
initial  configuration,  where  the  robot  needs  to  start  its  path  tracking,  are  stored,  the  robot  is 
able  to  perform  the  desired  motion.  This  is  because  the  reference  path  can  be  designed  as  a 
linked  list  which  stores  a  sequence  of  configurations  that  specify  the  reverse  path  and  the 
first  configuration  of  the  reversed  path  specifies  a  line  which  is  exactly  the  reference  line 
of  the  first  perpendicular  line  tracking.  Therefore,  in  implementing  mid-portion  motion 
planning,  the  data  structure  required  is  designed  as  a  linked  list  as  shown  in  Figure  1 1.3.  In 
the  Figure  1 1.3,  each  node  of  the  linked  list  holds  the  motion  instruction  for  one  region  of 
the  mid-portion  of  global  path. 


Figure  11.3:  Mid-portion  Motion  Data  Structure 


The  Region  ID  and  Exit  border  ID  are  the  type  of  integer  which  indicate  the 
current  region  and  it  exit  border.  The  Reference  path  of  the  structure  is  another  linked  list 
as  shown  in  Figure  11.1.  The  Sigma  is  a  type  of  double.  The  Entrance  config.  Initial 
conlig  and  Exit  config  are  all  type  of  configuration. 
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C.  LOCAL  MOTION  PLANNING  ALGORITHMS 

An  overall  algorithm  for  local  motion  planning  are  presented  in  Chapter  HI  (p.  42). 
The  mid-portion  motion  planning  and  end-portion  motion  planning  algorithms  are 
described  in  detailed  in  Chapter  IV  and  V  respectively.  Keep  in  mind  that  for  symmetric 
motion  planning,  the  final  motion  planning  is  conducted  in  reverse  manner.  All  related 
configurations  must  be  reversed  while  planning. 

D.  EXPERIMENT  RESULTS 

Most  of  motion  planning  algorithms  described  in  this  dissertation  have  been 
implemented  in  MML-11  and  tested  on  experimental  robot  Yamabico-11.  The  results 
shows  that  the  algorithms  are  practical  to  the  robot  motion  planning  and  motion  control. 
Figure  11.4  through  11.8,  show  the  trajectories  of  motion  executions  with  various  given 
start  and  goal  configurations  on  the  model  of  the  fifth  floor  in  Spanagel  Hall,  NPS. 


Figure  11.4:  Yamabico-ll  Motion  Planning  and  Execution  Results  #1 


Figure  11.5:  Yamabico-ll  Motion  Planning  and  Execution  Results  #2 


Figure  11.6:  Yamabico-ll  Motion  Planning  and  Execution  Results  #3 
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Figure  11.7:  Yamabico-ll  Motion  Planning  and  Execution  Results  #4 


Figure  11.8:  Yamabico-ll  Motion  Planning  and  Execution  Results  #5 


XII.  CONCLUSIONS 


This  dissertation  addressed  safe  motion  planning  using  layered  planning  approach 
which  divides  the  planning  task  into  global  path  planning  and  local  motion  planning. 
Among  the  researches  in  the  subject  of  motion  planning  for  mobile  robots,  the  contribution 
of  this  dissertation  provides  a  practical  solution  to  safe  motion  planning  problem  which  is 
a  great  step  in  promoting  motion  planning  in  the  real  world.  The  local  motion  planning  is 
accomplished  by  planning  motion  in  end-portion  and  mid-portion  of  the  global  path 
separately.  Under  the  safety  consideration,  the  smoothest  motion  is  achieved  by 
dynamically  computing  proper  smoothness  variable  for  reference  path  generation  and  path 
tracking.  This  dissertation  analyzed  various  region  situations  and  summarized  the  motion 
planning  of  any  single  region  into  six  rales  for  mid-portion  motion  panning.  These  rales 
simply  the  local  motion  planning  and  enhance  planning  efficiency. 

There  are  some  other  contributions  including:  (i)  design  of  real-time  robot 
operating  system  which  makes  robot  system  control  and  other  robot  sensing  devices  control 
work  on  the  inteirupt-driven  basis,  (ii)  incorporating  forerunner  simulation  into  real-time 
transition  point  calculation  to  make  robot  motion  control  more  flexible,  (iii)  development 
of  a  standard  high  level  robot  language  for  motion  planning  and  robot  system  control. 

In  addition,  the  steering  function  was  studied  intensively  through  many  simulations 
and  experiments.  The  characteristics  of  limitation  of  the  powerful  motion  planning  and 
control  tool  are  made  clear. 

Those  research  results  were  implemented  in  a  software  system,  MML-1 1  and  tested 
on  Yamabico-11.  The  experiment  results  show  that  the  algorithms  are  successful  in  robot 
motion  planning  and  motion  control. 
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APPENDIX  A.  FURTHER  UNDERSTANDING  OF  STEERING 

FUNCTION 

The  steering  function  as  Eq  A.  1  is  a  powerful  tool  for  a  mobile  robot  vehicle  performing 
smooth  path  tracking. 

tik 

—  =  -(AAk  +  BAQ  +  CAd)  (EqA.l) 

In  Eq  A.l,  A,  B,  and  C  are  positive  constants  which  are  related  to  the  smoothness  of 
robot’s  motion  [19].  The  meanings  of  these  variables,  Afc,  A6,  and  Ad,  are  as  follows:  Ak 
=  k-  k(i,  where  k is  vehicle’s  current  curvature  and  k^ the  desired  curvature.  A0  =  0  -  0(j, 
where  0  is  vehicle’s  current  orientation  and  0^  the  desired  orientation.  Ad  is  the  vehicle’s 
position  error.  How  the  steering  function  accomplishes  a  path  tracking  is  fully  described  in 
[19],  [21]  and  [7],  To  better  use  the  steering  function  in  motion  planning,  we  need  to 
understand  its  characteristics  deeply.  First  of  all,  we  investigate  the  limitation  of 
smoothness  which  will  be  applied  to  a  vehicle  in  path  tracking  using  the  steering  function. 
Then  we  look  for  a  possible  alternative  way  of  using  steering  function. 

A.  LIMITATION  OF  SMOOTHNESS  ON  LINE  TRACKING 

The  path  of  the  path  tracking  in  this  appendix  refer  to  a  straight  line.  Thus  we 
rephrase  the  term  path  tracking  as  line  tracking.  A  line  tracking  motion  starts  from  a 
configuration  =(p^,0  g,  kg).  The  goal  of  the  line  tracking  is  the  line  called  reference  Une, 
specified  by  a  configuration  =(Pr,Q  n  K)-  Since  the  line  is  a  straight  line,  it  has  a  constant 
curvature  k^  -  0.  The  constants  A,  B,  and  C  in  the  steering  function  are  determined  by  the 
smoothness  a  as  follows: 


k=  1 1  a 

(Eq  A.2) 

A  =  3k 

(EqA.3) 

B  =  3k2 

(Eq  A.4) 

C  =  k^ 

(Eq  A.5) 
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While  the  vehicle  is  tracking  a  line,  its  curvature  is  kept  updated  by  steering  function  in 
each  motion  cycle  [7]  until  the  vehicle’s  trajectory  converges  to  the  reference  line.  Thus  the 
smoothness  a  in  turns  determines  the  sharpness  of  the  tracking  trajectory.  Figure  A.l 
illustrates  the  change  of  sharpness  of  trajectories  when  different  smoothnesses  are  applied 
in  the  simulation.  In  fact,  we  see  that  the  smaller  the  value  of  smoothness  is,  the  sharper  the 
trajectory  will  be.  And  we  also  observed  that  the  larger  the  smoothness  is,  the  longer  the 
vehicle  travels  to  make  its  trajectory  converge  to  the  reference  path. 


Figure  A.l:  The  Sharpness  of  Tracking  Trajectory  Affected  by  Different 

Smoothness 

While  we  know  how  the  smoothness  affects  the  tracking  trajectory,  we  have  to  be 
aware  of  the  limitation  of  smoothness  that  can  be  applied  to  a  line  tracking.  It  is  important 
to  know  that  not  all  arbitrary  smoothness  can  be  used  in  all  cases  of  line  trackings.  As  a 
matter  of  fact,  there  is  no  upper  limit  for  the  smoothness  in  a  line  tracking,  even  though  an 
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oscillatory  trajectory  might  occur  if  the  applied  smoothness  is  too  large.  However,  we 
notice  that  a  lower  limit  of  smoothness  does  exist  in  using  the  steering  function  to  track  a 
line. 

By  observing  the  simulation  results  of  using  steering  function  in  line  tracking,  we 
found  that  the  lower  limit  of  smoothness  is  tightly  related  to  the  initial  Ad  which  is  the 
closest  distance  from  the  initial  configuration  to  the  reference  line.  Figure  A.2  illustrates 
the  results  of  simulations  using  some  critical  smoothnesses  in  line  tracking.  These 
simulation  were  set  by  tracking  positively  oriented  X-axis  of  the  global  frame  from  a 
configuration  with  initial  A0  =  0  and  Ad  =  100.  Because  the  initial  A6  =  0  means  the 
orientation  of  the  initial  configuration  and  the  reference  line  are  parallel,  this  type  of  line 
tracking  is  called  parallel  line  tracking.  From  these  parallel  line  tracking  simulation,  we 
found  that  the  smallest  smoothness  which  makes  the  tracking  trajectory  converge  to  the 
reference  line  is 

a  =  0.096  *  Adjnit  (Eq  A.6) 

where  Ad[^n  represents  the  initial  Ad.  If  a  smoothness  o  less  than  0.096  *  Ac/jnjt  is  applied, 
the  tracking  trajectory  never  converges  to  the  reference  line,  instead,  it  converges  to  a  line 
which  is  parallel  to  the  reference  line.  This  case  is  shown  as  the  trajectory  Jtj  in  Figure  A.2 
where  the  smoothness  is  o  =  0.095  *  Adi^i[- 

The  non-converging  (to  the  reference  line)  situation  happens  when  the  tracking 
trajectory  reaches  the  point  where  the  variables  of  the  steering  function  in  Eq  A.l  are  as 
follows: 

At  =  0;  (Eq  A.l) 

A6  =  2n7C  for  an  integer  n;  (Eq  A.  8) 

Ad^O; 

and  ^  =  0; 
ds 

Thus,  we  have 


BAO  +CAd  =  0 


(Eq  A.9) 


Then  by  substitute  Eq  A.9  with  Eq  A.4,  Eq  A.5  and  Eq  A.2,  we  have 


Ad 


BAQ 

C 


3k  xlrnz 


=  -6na7t; 


(EqA.10) 


As  the  point  on  the  tracking  trajectory  matches  Eq  A.7,  Eq  A.8  and  Eq  A.  10,  the  steering 
function  can  no  longer  steer  the  vehicle  to  the  reference  line.  Therefore  we  use  the  steering 
function  for  line  tracking,  the  proper  smoothness  must  be  carefully  chosen  to  avoid  the  non¬ 
convergence  from  happening. 


Figure  A.2;  The  Trajectories  of  Line  Tracking  with  Various  Smoothness 

In  addition  to  the  worst  case  described  above,  %2  ^3  Figure  A.2  demonstrate 

other  undesirable  line  tracking  trajectories.  Although  they  converge  to  the  reference  line 
eventually,  the  trajectories  travel  backward  at  first  half.  On  the  contrary,  we  consider  714  as 
a  desirable  tracking  trajectory  because  it  always  travels  in  the  direction  of  the  reference  line 
heading.  From  these  simulations,  we  conclude  that  for  parallel  line  tracking  the  minimum 
desirable  smoothness  (which  makes  a  desirable  converging  trajectory)  is: 
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a  =  0.18  *  Adjnit  (EqA.ll) 

The  Figure  A.2  demonstrates  only  the  simulation  results  of  a  parallel  line  tracking 

where  we  set  initial  A0  =  0.  When  the  initial  A0  is  not  zero,  this  variable  is  also  found 

involved  in  the  determination  of  smoothness.  Fortunately,  since  in  any  cases  both  initial  Ad 

and  A0  are  known,  by  using  well-designed  simulation,  the  minimum  smoothness  can  be 

computed.  Table  A.l  and  Table  A.2  list  some  simulation  data  showing  the  relationship 

between  initial  Ad  and  its  minimum  desirable  smoothness  a.  From  the  tables  we  found  that 

for  all  different  initial  orientations,  the  minimum  smoothness  is  less  than  0.22  *  Adi^if  We 

will  take  this  Max-Min  smoothness  in  some  cases  in  Initial-portion  Motion  Planning. 

Table  A.l:  The  Relationship  among  Distances  and  Smoothness  in  Line  Tracking  with 
Negative  A0  (in  degree),  and  Corresponding  Minimum  desirable  Smoothness  a 


Initial  A0 

^init 

a 

L 

LI  <5 

0 

100.0 

18.0 

177.3 

0.18 

9.85 

15 

100.0 

19.0 

191.6 

0.19 

10.09 

30 

100.0 

19.0 

191.7 

0.19 

10.09 

45 

100.0 

20.0 

2.04.6 

0.20 

10.23 

60 

100.0 

20.0 

203.2 

0.20 

10.16 

75 

100.0 

21.0 

214.4 

0.21 

10.21 

90 

100.0 

21.0 

211.9 

0.21 

10.09 

105 

100.0 

22.0 

222.3 

0.22 

10.10 

120 

100.0 

22.0 

219.1 

0.22 

9.96 

135 

100.0 

22.0 

216.3 

0.22 

9.83 

150 

100.0 

22.0 

213.8 

0.22 

9.72 

165 

100.0 

22.0 

211.6 

0.22 

9.62 

180 

100.0 

22.0 

209.7 

0.22 

9.53 
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Table  A.2:  The  Relationship  among  Distances  and  Smoothness  in  Line  Tracking  with 
Negative  A0  (in  degree),  and  Corresponding  Minimum  desirable  Smoothness  a 


Initial  A0 

^init 

a 

L 

Lie 

-15 

100.0 

18.0 

175.7 

0.18 

9.76 

-30 

100.0 

18.0 

173.1 

0.18 

9.62 

^5 

100.0 

18.0 

169.9 

0.18 

9.44 

-60 

100.0 

18.0 

165.8 

0.18 

9.21 

-75 

100.0 

18.0 

161.4 

0.18 

8.97 

-90 

100.0 

22.0 

200.0 

0.22 

9.09 

-105 

100.0 

19.0 

163.4 

0.19 

8.60 

-120 

100.0 

16.0 

123.9 

0.16 

7,75 

-135 

100.0 

15.0 

106.5 

0.15 

7.10 

-150 

100.0 

13.0 

70.2 

0.13 

5.40 

-165 

100.0 

12.0 

41.3 

0.12 

3.44 

B.  TWO-WAY  LINE  TRACKING 

Although  the  steering  function  is  designed  under  the  assumption  of  -7C  /2  <  A0  < 
Jc/2,  it  is  applicable  to  all  orientation  differences.  For  a  given  initial  configuration  and  a 
reference  line  with  a  proper  smoothness,  the  line  tracking  can  be  performed  smoothly  and 
its  trajectory  is  unique.  Figure  A.3  shows  the  trajectories  of  tracking  a  reference  line 
(positively  oriented  X-axis)  from  the  initial  configurations  which  are  different  in  their 
orientations  only.  The  trajectory  7ti  starts  the  line  tracking  from  the  initial  configuration  ((0, 
100),  -180,  0)  and  trajectory  ttg  starts  from  ((0,  100),  135,  0).  The  orientations  in  those 
initial  configurations  vary  from  -  7t  to  37t  /  4.  In  steering  function  point  of  view,  the 
orientation  differences  are  -  7t  <  A0  <  3ji  /  4.  It  looks  nature  to  have  the  trajectories  as  Figure 
A.3  for  this  line  tracking  example.  However,  in  some  cases,  we  may  expect  the  vehicle  to 
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travel  a  trajectory  different  from  the  one  shown  in  Figure  A.3  (starting  at  the  same  initial 
configuration  and  tracking  the  same  reference  line).  For  instance,  the  tracking  trajectory  JCg 
in  Figure  A.3  starts  with  a  clockwise  motion  at  the  beginning.  If  there  is  no  way  to  avoid  a 
possible  collision  by  traveling  this  clockwise  trajectory,  we  may  try  a  possible 
counterclockwise  tracking  trajectory.  The  Figure  A.4  illustrate  the  idea  of  clockwise  and 
counterclockwise  tracking  trajectory.  In  Figure  A.4,  clockwise  trajectory  ;t^  is  exactly  the 
same  as  Tt^  in  Figure  A.3.  The  method  we  use  to  tracking  a  line  either  clockwise  or 
counterclockwise  ins  called  Two-way  Line  Tracking. 


Figure  A.3:  The  Trajectories  of  Line  Tracking  from  Different  Orientation 

The  steering  function  of  Eq  A.l  is  capable  for  the  Two-way  Line  Tracking.  We 
notice  that  the  line  tracking  trajectories  in  Figure  A.3  have  their  initial  orientations 
expressed  as  -tc  <  9  <  tc.  Therefore,  when  tracking  a  line  from  those  configurations,  we  have 
the  orientation  difference  also  as  -7C  <  A9  <  7t.  As  we  know,  any  orientation  difference  can 
be  normalized  to  this  orientation  interval.  We  considered  a  line  tracking  with  the 
normalized  orientation  difference  as  a  normal  line  tracking.  Besides  the  normal  one,  if  the 
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initial  orientation  difference  is  expressed  by  the  following  rule, 
if  A0  <  0  ,  A0  =  A9  +  2tz. 
else  A0  =  A0  -  2jt. 

the  counterclockwise  tracking  trajectory  as  in  Figure  A.4  is  produced.  We  consider 
the  line  tracking  with  this  orientation  difference  as  an  alternative  line  tracking.  The  Figure 
A.5,  A.6  and  A.7  demonstrate  some  more  examples  of  Two  way  Line  Trackings. 
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Figure  A.7:  Clockwise  and  Counterclockwise  Tracking  Trajectory  with 

Initial  A0  =  -  7t  /  2 
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APPENDIX  B.  DYNAMIC  MEMORY  MANAGEMENT  STRATEGY 
A.  INTRODUCTION 

The  dynamic  memory  management  involves  allocation  and  deallocation  of  memory 
blocks  with  requested  size.  If  they  are  not  handled  properly,  the  memory  fragmentation  will 
eventually  cause  inefficient  memory  use.  Without  any  management  strategy,  we  may 
allocate  memory  blocks  to  the  user  sequentially  as  request  and  deallocate  them  in  the  order 
of  deallocation  request.  For  example,  if  we  have  100  k  bytes  memory  available  for  dynamic 
memory  use,  and  the  request  for  the  memory  are: 

a.  Request  a  block  of  size  20  k. 

b.  Request  a  block  of  size  30  k. 

c.  Request  a  block  of  size  15  k. 

d.  Request  a  block  of  size  25  k. 

e.  Release  the  block  in  request  b. 

f.  Request  a  block  of  size  25  k. 

g.  Request  a  block  of  size  10  k. 

h.  Release  the  block  in  request  a. 

i.  Request  a  block  of  size  10k. 

j.  Release  the  block  in  request  c. 

k.  Release  the  block  in  request  d. 

l.  Request  a  block  of  size  20  k. 

m.  Request  a  block  of  size  20  k. 

The  request  a-  d  will  result  the  memory  partitioned  as  Figure  B.l.  There  is  only  one 
memory  block  free  with  size  10  k.  The  request  e-m  will  result  the  memory  partitioned  as 
Figure  B.2.  There  are  several  free  blocks  with  total  size  35  k  after  request  1.  However,  when 
request  m  comes,  system  will  not  be  able  to  allocate  any  memory  block  to  this  request,  even 
though  the  size  of  total  available  free  memory  is  large  than  the  size  of  current  request. 
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Figure  B.2:  Dynamic  Memory  Allocation  Partition 
after  Request  e-m 
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After  more  allocation  and  deallocation  operations,  the  dynamic  memory  space  may 
be  partitioned  into  many  small  blocks  either  consecutive  or  separate.  This  is  not  desirable 
because  it  will  eventually  make  it  impossible  to  fill  the  legitimate  request  of  a  user  needing 
one  larger  memory  block.  The  request  m  is  an  example.  To  remedy  this  problem,  we  must 
have  a  method  that  allows  a  block  being  returned  to  the  available  list  to  be  coalesced  with 
any  other  block(s)  in  the  list  that  are  the  physical  neighbor(s)  of  the  returning  block. 
Therefore  a  buddy  system,  which  can  coalesce  neighboring  free  blocks  to  form  a  larger 
memory  block  available,  is  needed  for  dynamic  memory  allocation  management. 

B.  BUDDY  SYSTEMS 

The  general  purpose  of  a  buddy  system  is  to  provide  a  method  that  designates  one 
or  two  buddy  blocks  for  each  block  when  partitioning  the  memory.  The  buddy  blocks  have 
to  reside  right  next  to  its  corresponding  block.  When  a  block  is  to  be  released,  its  buddy 
block  will  be  checked  to  see  if  it’s  available  for  coalescence.  If  the  coalescence  is  possible, 
the  two  blocks  will  be  returned  as  one  larger  block  to  available  memory.  The  coalescence 
will  be  performed  recursively  whenever  buddy  block  is  available.  To  determine  the  buddy 
of  a  given  block,  it  is  necessary  to  impose  certain  restrictions  on  block  sizes  and/or  to  store 
a  fair  amount  of  bookkeeping  information  in  each  block. 

The  most  common  buddy  systems  are  [28]: 

•  Binary  buddy  system. 

•  Fibonacci  buddy  system. 

•  Boundary  tag  buddy  system. 

Among  them,  the  Fibonacci  buddy  system  is  the  most  popular  one  for  the  following 
reason.  Firstly  it  allows  for  a  greater  variety  of  possible  block  sizes  in  a  given  amount  of 
memory  than  Binary  buddy  system.  Second,  the  Fibonacci  buddy  system  provides  an 
efficient  method  to  allocate  and  deallocate  memory  blocks. 

The  basic  requirement  of  buddy  system  is  that  any  block  size  (except  the  smallest 
one)  must  result  from  coalescing  two  smaller  blocks,  so  that  the  addresses  of  the  buddies 
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can  be  easily  computed  in  allocation  and  deallocation  operation.  In  Fibonacci  sequence,  the 
ith  member  of  the  sequence  is  recursively  defined  as 
Fi  =  F(i-l)  +  P(i-2)  fori  >2 

Therefore  using  Fibonacci  sequence  as  the  memory  splitting  strategy  meet  the  requirement 
of  buddy  system.  The  Fibonacci  sequence  can  be  stored  in  an  array  of  desired  data  structure 
which  is  used  to  indicate  the  free  memory  blocks  with  size  corresponding  to  the  Fibonacci 
number.  In  the  array,  each  element  may  contain  a  pointer  that  points  to  the  available  block 
of  corresponding  size. 

Let’s  take  an  example  to  demonstrate  how  the  Fibonacci  buddy  system  works. 
Suppose  we  have  Fj  =  1,  F2  =  2,  then  the  members  of  the  Fibonacci  sequence  are: 

1,  2,  3,  5,  8, 13,  21,  34, ... 

And  suppose  that  we  are  managing  21k  memory  and  are  faced  with  the  following  requests 
for  storage: 

a.  Request  for  7k. 

b.  Request  for  7k. 

c.  Request  for  2k. 

d.  Release  7k  of  request  b. 

e.  Release  2k  of  request  c. 

f.  Release  7k  of  request  a. 

Figure  B.3  to  B.9  illustrate  the  allocation,  deallocation  and  resulting  coalescing  that  would 
occur  as  these  requests  were  processed.  In  each  figure,  the  rectangle  in  the  center  illustrates 
the  memory  block(s)  status.  The  blank  block  means  it  is  a  free  memory  block  while  the 
shaded  block  indicates  an  occupied  space.  On  the  left,  a  tree  with  a  number  in  each  circle 
illustrates  the  splitting  of  Fibonacci  sequence  after  an  allocation  or  a  deallocation  request 
is  processed  starting  with  the  size  of  entire  available  memory  space.  Each  node  of  leaves 
of  the  tree  corresponds  to  a  memory  block.  On  the  right,  the  array  uses  pointers  that  point 
to  free  memory  blocks  of  corresponding  size. 
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Figure  B.7:  Dynamic  Memory  Allocation  Partition 
after  Request  (d)  Processed  -  8k  Deallocated  But 
No  Coalescing 


Figure  B.8:  Dynamic  Memory  Allocation  Partition 
after  Request  (e)  Processed  -  2  k  Deallocated  and 
Coalescing  Occurs  Up  to  13  k  Block 
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21 


Free 


Figure  B.9:  Dynamic  Memory  Allocation  Partition 
after  Request  (f)  Processed  -8k  Deallocated  and 
Coalescing  Occurs  -  One  Free  21  k  Block  Results 


Naturally,  some  additional  overhead  is  required  when  the  Fibonacci  buddy  system 
is  used.  First,  depending  on  the  implementation  scheme,  it  may  be  necessary  to  store  the 
Fibonacci  numbers  themselves  in  an  array  to  allow  quick  access  to  data  necessary  to 
allocate,  split,  and  coalesce  blocks.  Second,  it  is  necessary  to  store  some  bookkeeping  data 
within  each  block  so  that  it  tells  the  block’s  status  (free  or  not),  size,  links  to  other  blocks, 
and  depth  of  being  left  buddy  to  other  blocks  as  Figure  B.IO. 


Free 

flag 

Size 

Left 

link 

Right 

link 

Left 

buddy 

Not  used  if 
block  active 

count 

Memory  block  available 


Figure  B.IO:  Bookkeeping  Information  in  Block  for  Fibonacci 
Buddy  System 
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APPENDIX  C.  USER  PROGRAM  EXAMPLES 


Program  Name  :user.c-#l 

Purpose  :  For  Model  Based  Motion  Planning  demo. 

Parameters  :  void 

Returns  :  void 

Comments  :  4/1/95  Chien-Liang  Chuang 

#include  “user.h” 

void  setconfigO; 

CONHGURATION  chooseConfigO; 

CONFIGURATION  q,  qO,  ql,  q2,  q3,  q4,  q5,  q6,  q7,  q8,  q9,  qlO, 
qll,  ql2,  ql3,  ql4,  ql5,  ql6,  ql7,  ql8,  goal; 

void 

user() 

{ 

mt  go; 
double  theta; 

PATH_ELEMENT  path; 


createModelO; 

go  =  1; 
setconfigO; 

printf(“\nSelect  0-18  for  default  START,99  for  input:  “); 
q  =  chooseConfigO; 
setRobotConfiglmm(q) ; 

printf(“\nSelect  0-18  for  default  GOAL,  99  for  input ;  “); 

goal  =  chooseConfigO; 

setLinVelImm(30.0); 

MotionLog(NULL,  20, 0); 
while  (go)  { 


gotolmm(goal); 

path  =  getPathElementO; 
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while  (path.pathType.mode  ==  STOPMODE) 
path  =  getPathElementO; 

while  (path.pathType.mode  !=  STOPMODE) 
path  =  getPathElementO; 

q  =  getRobotConfigO; 

printf(“\n\nGo  to  somewhere  else?  1/0”); 

go  =  GetIntO; 

if  (go  !=0) 

{ 

printf(“\nSelect  0-18  for  GOAL, 99  for  input :  “); 
goal  =  chooseConfigO; 

} 

} 

} 


Function  :  setconfigO  to  set  default  configuration 

*1^  j 

void 

setconfigO 

{ 


qO  =  defineConfig(200.0, 214.0, 0.0, 0.0); 
ql  =  defineConfig(125.0, 345.0,  HPI,  0.0); 
q2  =  defineConfig(50.0, 657.0,  PI,  0.0); 
q3  =  defineConfig(125.0,  872.0,  HPI,  0.0); 
q4  =  defineConfig(125.0, 2000.0,  -HPI,  0.0); 
q5  =  defineConfig(50.0, 1862.0,  PI,  0.0); 
q6  =  defineConfig(50.0,  2148.0,  PI,  0.0); 
q7  =  defineConfig(50.0, 2433.0,  PI,  0.0); 
q8  =  defineConfig( 190.0, 1980.0, 0.0, 0.0); 
q9  =  defineConfig( 190.0, 1390.0, 0.0, 0.0); 
qlO  =  defineConfig(555.0,  867.0, 0.0, 0.0); 
qll  =  defineConfig(800.0, 743.0, 0.0, 0.0); 
ql2  =  defineConfig(707.0, 720.0,  -HPI,  0.0); 
ql3  =  defineConfig(417.0, 720.0,  -HPI,  0.0); 
ql4  =  defineConfig(417.0, 546.0,  -HPI,  0.0); 
ql5  =  defineConfig(706.0,  546.0,  -HPI,  0.0); 
ql6  =  defineConfig(-93.0, 2138.0,  PI,  0.0); 
ql7  =  defineConfig(-278.0, 2121.0,  PI,  0.0); 
ql8  =  defineConfig(-505.0,  2150.0,  PI,  0.0); 
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Function  :  chooseConfigO  for  selecting  a  configuration 
CONFIGURATION 


chooseConfigO 

{ 

int  set; 
double  X,  y,  t; 
CONFIGURATION  q; 


set  =  GetIntO; 

switch  (set){ 

case  0;  q  =  qO;  break; 
case  1 :  q  =  ql;  break; 
case  2:  q  =  q2;  break; 
case  3;  q  =  q3;  break; 
case  4:  q  =  q4;  break; 
case  5:  q  =  q5;  break; 
case  6:  q  =  q6;  break; 
case  7 :  q  =  qV;  break; 
case  8:  q  =  q8;  break; 
case  9:  q  =  q9;  break; 
case  10:  q  =  qlO;  break; 
case  1 1 :  q  =  ql  1;  break; 
case  12:  q  =  ql2;  break; 
case  13:  q  =  ql3;  break; 
case  14;  q  =  ql4;  break; 
case  15:  q  =  ql5;  break; 
case  16:  q  =  ql6;  break; 
case  17:  q  =  ql7;  break; 
case  18:  q  =  ql8;  break; 

default : 

printf(‘VInput  configuration :  “); 
printf(“  x  =  “); 

X  =  GetRealO; 
printf(“  y  =  “); 
y  =  GetRealO; 
printf(“  theta  =  “); 
t  =  GetRealO; 
t  =  t*DAR; 

q  =  defineConfig(x,  y,  t,  0.0); 
break; 

} 

retum(q); 
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Program  Name  :user.c-#2 
Purpose  :  For  Sonar  Testing 

Parameters  :  void 

Returns  :  void 

Comments  :  4/1/95  Chien-Liang  Chuang 

#include  “user.h” 

#defineFREQ  1 

int  SONARNUM,  DATATYPE; 
int  type; 

void  userlQ; 
void  user2(); 
void  userSO; 


void  user() 

{ 

int  selection; 

printf(“\n  Enter  1  for  Stationary  testing  “); 
printf(“\n  Enter  2  for  Moving  (line)  testing  “); 
printf(‘^n  Enter  3  for  Moving  (rotate)  testing  “); 
selection=GetInt(); 

printf(“\n  Input  sonar  # :  “); 

SONARNUM=GetInt(); 

printf(“\n  Input  1  for  logging  RAW-data,  2  for  logging  GLOBAL-data :  “); 

type=GetInt(); 

if  (type==l) 

DATATYPE  =  SONAR_RAW; 
else 

DATATYPE  =  SONAR_GLOBAL; 

switch  (selection) 

{ 

case  1; 

userl(); 

break; 

case  2: 

user2(); 

break; 

case  3: 

user3(); 

break; 

default: 
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break; 

} 


} 


/^L*  *1^  *1a  *>1a  aI*  ^sl>  *>1<*  si*  4*  4i*  ^1*  si*  'st'  >1*  si*  si*  sis  si*  si*  si*  si*  si*  ^*  si*  si*  si*  si*  si*  ^*  4*  si*  ^*  si*  si*  slf  *1*  si*  sis  sl^  si*  sl^  st  sl^  stf  sis  sis  si*  si?  sJtf 

/  ^  Jj>  Jjv  ?f»  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  *T»  ^  ^  #yi  ^  ^  ^  >|w  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  >]>  ^  ^  ^  ^  ^  ^  *j»  ^  #7*  *js  ^  *y» 

Function  :  userlQ  for  stationary  testing 

*1»  xL»  «X»  «!•  ^L»  ^1*  ^L*  ^X*  ^I*  «1*  ^Sa  «1*  *1*  si*  si*  ^*  si*  si*  ^*  *1*  si*  ^1*  si*  si*  si*  si*  si*  ^r*  ^1*  *1*  si*  *1*  ^1*  *1*  ^1*  si*  *1*  ^1*  si*  si*  ^1*  ^1*  si*  si*  si*  *1*  sl*  sl*  sl*  sl?  sl?  sLf  st*  sl?  st?  sl^  slf  / 

JjC  ^fZ  ?|C  <J»  ^  Pji  <f5  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  *X*  ^  ^  ^  ^  ^  ^  ^  *1^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  ^  *r“  *T*  *T*  *T^  *T*  **•  *T*  *t*  *r*  *!•  *x*  / 

void  userlQ 

{ 


double  distance; 
int  cnt  =  0; 


EnableSonar(SONARNUM); 

SonarLog(FREQ,0,SONARNUM, DATATYPE); 

waitMS(30); 

while  (++cnt  <=  50) 

{ 

distance  =  Sonar(SONARNUM); 

printf(“\nSonar  Range  is  %f’, distance); 

waitMS(50); 

} 


*1^  *1^  *1^  *1'^  *1^  *f*  *^^  *J*  *1^  *1'^  *{"' 

Function  :  user2()  for  moving  (line)  testing 

void  user2() 

{ 


double  dist,  speed; 
CONFIGURATION  q,p; 


printfCV  Input  desired  speed :  “); 
speed  =  GetRealO; 
setLinVellmm(speed); 

printf(“\n  Input  traveling  distance  ;  “); 
dist  =  GetRealO; 


p  =  defineConfig(0.0, 0.0, 0.0, 0.0); 


q  =  defineConfig(dist,  0.0, 0.0, 0.0); 

setRobotConfig(p); 

EnableSonar(SONARNUM); 

SonarLog(5,0,SONARNUM,DATATYPE); 

bline(q); 

} 


jfc  jjc  Jjc  jjc  jjc  5|c  jjc  jjc  Jjc  5jc  Jjc  5|C  Jjc  Sjc  3^  ^'C  5{c  5jf> 

Function  :  user3()  for  moving  (rotate)  testing 

itf  ^1*  ^If  ^1*  Klf  s2i>  ^2^  >1^  sL*  ^1#  «1^  «sL>  «.!»  >1^  «ii^  «li»  •df  «X*  «!••  / 

'f*  'r*  *7^  ^T*  'I'  "T*  ^r*  ^T*  ^T*  't*  ^4^  'T*  ^iS  ^1*  'I*  ^T*  / 

void  user3() 

f 


{ 


CONFIGURATION  q; 


q  =  defineConfig(0.0, 0.0, 0.0, 0.0); 

setRobotConfig(q); 

EnableSonar(SONARNUM); 

SonarLog(FREQ,0,SONARNUM,DATATYPE); 

waitMS(30); 

Rotate(PI); 


} 
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